#include "include/model_functions.hpp"

//#include <eigen3/Eigen/Dense>
//#include <eigen3/Eigen/QR>
//#include <mutex>
//#include "leg_config.hpp"

//using Eigen::MatrixXd;

void update_q(double theta1, double theta2, double theta3, double theta4, double theta5, Eigen::Matrix<double, 5, 1> &q) {
    
    q << theta1,
        theta2,
        theta3,
        theta4,
        theta5;
}

void update_q_dot(double theta1dot, double theta2dot, double theta3dot, double theta4dot, double theta5dot,  Eigen::Matrix<double, 5, 1> &q_dot) {
    
    q_dot << theta1dot,
            theta2dot,
            theta3dot,
            theta4dot,
            theta5dot;
}

void update_B(double theta_1, double theta_2, double theta_3, double theta_4, double theta_5, double thetadot1, double theta2dot, double theta3dot, double theta4dot, double theta5dot, Eigen::Matrix<double, 5, 5> &B, leg_config &config) {
    double temp_1 = config.Izz_hip1 + config.Izz_hip2 + config.m_foot*pow(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)), 2) + config.m_foot*pow(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)), 2) + config.m_hip1*pow(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x_com*sin(theta_1)*cos(theta_2) + config.l3_y_com*cos(theta_1) + config.l3_z_com*sin(theta_1)*sin(theta_2), 2) + config.m_hip1*pow(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x_com*cos(theta_1)*cos(theta_2) + config.l3_y_com*sin(theta_1) - config.l3_z_com*sin(theta_2)*cos(theta_1), 2) + config.m_hip2*pow(config.l2_x_com*sin(theta_1) + config.l2_y_com*cos(theta_1), 2) + config.m_hip2*pow(-config.l2_x_com*cos(theta_1) + config.l2_y_com*sin(theta_1), 2) + config.m_ll*pow(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)), 2) + config.m_ll*pow(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)), 2) + config.m_ul*pow(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)), 2) + config.m_ul*pow(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)), 2) - (-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*sin(theta_2) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) - (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_2) + (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*sin(theta_3)*cos(theta_2) + (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*cos(theta_2)*cos(theta_3);

    double temp_2 = config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*cos(theta_1) + config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*sin(theta_1) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x_com*sin(theta_1)*cos(theta_2) + config.l3_y_com*cos(theta_1) + config.l3_z_com*sin(theta_1)*sin(theta_2))*cos(theta_1) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x_com*cos(theta_1)*cos(theta_2) + config.l3_y_com*sin(theta_1) - config.l3_z_com*sin(theta_2)*cos(theta_1))*sin(theta_1) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*cos(theta_1) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*sin(theta_1) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*cos(theta_1) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*sin(theta_1) + (config.Izx_hip1*sin(theta_1) + config.Izy_hip1*cos(theta_1))*cos(theta_1) - (config.Izx_hip1*cos(theta_1) - config.Izy_hip1*sin(theta_1))*sin(theta_1) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*sin(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*cos(theta_1) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2))*cos(theta_1) + ((-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*sin(theta_1)*cos(theta_2) + (-config.Ixy_ul*sin(theta_2) + config.Izy_ul*cos(theta_2))*cos(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_1)*sin(theta_2))*cos(theta_1) - ((-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*cos(theta_1)*cos(theta_2) - (-config.Ixy_ul*sin(theta_2) + config.Izy_ul*cos(theta_2))*sin(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_2)*cos(theta_1))*sin(theta_1);

    double temp_3 = config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))) - (-(-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*sin(theta_2) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*cos(theta_2))*sin(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) - ((-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2))*sin(theta_2) - (-(-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_2) + (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*sin(theta_3)*cos(theta_2) + (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*cos(theta_2)*cos(theta_3))*sin(theta_2) + ((-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*sin(theta_1)*cos(theta_2) + (-config.Ixy_ul*sin(theta_2) + config.Izy_ul*cos(theta_2))*cos(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_1)*sin(theta_2))*sin(theta_1)*cos(theta_2) + ((-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*cos(theta_1)*cos(theta_2) - (-config.Ixy_ul*sin(theta_2) + config.Izy_ul*cos(theta_2))*sin(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_2)*cos(theta_1))*cos(theta_1)*cos(theta_2);

    double temp_4 = config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) - ((-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2))*sin(theta_2) - (-(-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_2) + (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*sin(theta_3)*cos(theta_2) + (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*cos(theta_2)*cos(theta_3))*sin(theta_2);

    double temp_5 = config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) - ((-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2))*sin(theta_2);

    double temp_6 = -config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_2) + config.Iyz_hip1*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2)) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_2) + config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*cos(theta_1) + config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*sin(theta_1) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x_com*sin(theta_1)*cos(theta_2) + config.l3_y_com*cos(theta_1) + config.l3_z_com*sin(theta_1)*sin(theta_2))*cos(theta_1) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x_com*cos(theta_1)*cos(theta_2) + config.l3_y_com*sin(theta_1) - config.l3_z_com*sin(theta_2)*cos(theta_1))*sin(theta_1) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*cos(theta_1) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*sin(theta_1) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*cos(theta_1) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*sin(theta_1) - (config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_2) - (config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_3)*cos(theta_2) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_2)*cos(theta_3);

    double temp_7 = config.m_foot*pow(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2), 2)*pow(sin(theta_1), 2) + config.m_foot*pow(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2), 2)*pow(cos(theta_1), 2) + config.m_foot*pow(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5), 2) + config.m_hip1*pow(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2), 2)*pow(sin(theta_1), 2) + config.m_hip1*pow(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2), 2)*pow(cos(theta_1), 2) + config.m_hip1*pow(config.l3_x_com*cos(theta_2) + config.l3_z_com*sin(theta_2), 2) + config.m_ll*pow(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2), 2)*pow(sin(theta_1), 2) + config.m_ll*pow(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2), 2)*pow(cos(theta_1), 2) + config.m_ll*pow(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4), 2) + config.m_ul*pow(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3), 2)*pow(sin(theta_1), 2) + config.m_ul*pow(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3), 2)*pow(cos(theta_1), 2) + config.m_ul*pow(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3), 2) + (config.Iyx_hip1*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1) + config.Iyy_hip1*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1))*cos(theta_1) - (config.Iyx_hip1*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1) - config.Iyy_hip1*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1))*sin(theta_1) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*cos(theta_1) - ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*sin(theta_1) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*cos(theta_1) - ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*sin(theta_1) + (config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1)*cos(theta_2) + config.Iyy_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1)*sin(theta_2))*cos(theta_1) - (config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1)*cos(theta_2) - config.Iyy_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_2)*cos(theta_1))*sin(theta_1);

    double temp_8 = config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_2) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*cos(theta_1) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*sin(theta_1) + config.m_ul*(-config.l4_y_com*cos(theta_3) + config.l4_z_com*sin(theta_3))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3))*cos(theta_2) - (-config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_2) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_2))*sin(theta_2) - (-(config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) - (-(config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_3)*cos(theta_2) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_2)*cos(theta_3))*sin(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1)*cos(theta_2) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*sin(theta_1)*cos(theta_2) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*cos(theta_1)*cos(theta_2) + (config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1)*cos(theta_2) + config.Iyy_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1)*sin(theta_2))*sin(theta_1)*cos(theta_2) + (config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1)*cos(theta_2) - config.Iyy_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_2)*cos(theta_1))*cos(theta_1)*cos(theta_2);

    double temp_9 = config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_2) - (-(config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) - (-(config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_3)*cos(theta_2) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_2)*cos(theta_3))*sin(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1)*cos(theta_2) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*sin(theta_1)*cos(theta_2) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*cos(theta_1)*cos(theta_2);

    double temp_10 = config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) - (-(config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1)*cos(theta_2);

    double temp_11 = config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))) - (config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_2) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2)*cos(theta_3);

    double temp_12 = config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_2) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*cos(theta_1) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*sin(theta_1) + config.m_ul*(-config.l4_y_com*cos(theta_3) + config.l4_z_com*sin(theta_3))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3))*cos(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*cos(theta_1) + ((config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*sin(theta_2))*cos(theta_1) - ((config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_2)*cos(theta_1))*sin(theta_1);

    double temp_13 = config.m_foot*pow(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)), 2) + config.m_foot*pow(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)), 2) + config.m_foot*pow(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5), 2)*pow(cos(theta_2), 2) + config.m_ll*pow(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)), 2) + config.m_ll*pow(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)), 2) + config.m_ll*pow(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4), 2)*pow(cos(theta_2), 2) + config.m_ul*pow(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)), 2) + config.m_ul*pow(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)), 2) + config.m_ul*pow(-config.l4_y_com*cos(theta_3) + config.l4_z_com*sin(theta_3), 2)*pow(cos(theta_2), 2) - (-(config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_2) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_2))*sin(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) - ((-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*sin(theta_2) - (-(config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2)*cos(theta_3))*sin(theta_2) + ((config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*sin(theta_2))*sin(theta_1)*cos(theta_2) + ((config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_2)*cos(theta_1))*cos(theta_1)*cos(theta_2);

    double temp_14 = config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*pow(cos(theta_2), 2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) - ((-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*sin(theta_2) - (-(config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2)*cos(theta_3))*sin(theta_2);

    double temp_15 = config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) - ((-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*sin(theta_2);

    double temp_16 = config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2)*cos(theta_3);

    double temp_17 = config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*cos(theta_1);

    double temp_18 = config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*pow(cos(theta_2), 2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) - ((-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*sin(theta_2) - (-(config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2)*cos(theta_3))*sin(theta_2);

    double temp_19 = config.m_foot*pow(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)), 2) + config.m_foot*pow(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)), 2) + config.m_foot*pow(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5), 2)*pow(cos(theta_2), 2) + config.m_ll*pow(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)), 2) + config.m_ll*pow(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)), 2) + config.m_ll*pow(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4), 2)*pow(cos(theta_2), 2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) - ((-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*sin(theta_2) - (-(config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2)*cos(theta_3))*sin(theta_2);

    double temp_20 = config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) - ((-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*sin(theta_2);

    double temp_21 = config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2);

    double temp_22 = config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1);

    double temp_23 = config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) - ((-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*sin(theta_2);

    double temp_24 = config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) - ((-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*sin(theta_2);

    double temp_25 = config.m_foot*pow(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)), 2) + config.m_foot*pow(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)), 2) + config.m_foot*pow(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5), 2)*pow(cos(theta_2), 2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) - ((-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*sin(theta_2);


    B << temp_1, temp_2, temp_3, temp_4, temp_5,
                  temp_6, temp_7, temp_8, temp_9, temp_10,
                  temp_11, temp_12, temp_13, temp_14, temp_15,
                  temp_16, temp_17, temp_18, temp_19, temp_20,
                  temp_21, temp_22, temp_23, temp_24, temp_25;
}

void update_C(double theta_1, double theta_2, double theta_3, double theta_4, double theta_5, double thetadot1, double thetadot2, double thetadot3, double thetadot4, double thetadot5, Eigen::Matrix<double, 5, 1> &C, leg_config &config) {
    double temp_1 = -0.5L*((config.m_foot*(config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l2_x*cos(theta_1) - config.l2_y*sin(theta_1) + config.l3_x*cos(theta_1)*cos(theta_2) - config.l3_y*sin(theta_1) + config.l3_z*sin(theta_2)*cos(theta_1) + config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l5_x*cos(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_x_com*cos(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l4_z*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + (-config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot5 + (config.m_foot*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(2*config.l2_x*cos(theta_1) - 2*config.l2_y*sin(theta_1) + 2*config.l3_x*cos(theta_1)*cos(theta_2) - 2*config.l3_y*sin(theta_1) + 2*config.l3_z*sin(theta_2)*cos(theta_1) + 2*config.l4_x*cos(theta_1)*cos(theta_2) + 2*config.l4_y*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + 2*config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + 2*config.l5_x*cos(theta_1)*cos(theta_2) + 2*config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - 2*config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + 2*config.l6_x_com*cos(theta_1)*cos(theta_2) + 2*config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - 2*config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(2*config.l2_x*sin(theta_1) + 2*config.l2_y*cos(theta_1) + 2*config.l3_x*sin(theta_1)*cos(theta_2) + 2*config.l3_y*cos(theta_1) + 2*config.l3_z*sin(theta_1)*sin(theta_2) + 2*config.l4_x*sin(theta_1)*cos(theta_2) + 2*config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - 2*config.l4_z*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) + 2*config.l5_x*sin(theta_1)*cos(theta_2) - 2*config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - 2*config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + 2*config.l6_x_com*sin(theta_1)*cos(theta_2) - 2*config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - 2*config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_hip1*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x_com*sin(theta_1)*cos(theta_2) + config.l3_y_com*cos(theta_1) + config.l3_z_com*sin(theta_1)*sin(theta_2))*(2*config.l2_x*cos(theta_1) - 2*config.l2_y*sin(theta_1) + 2*config.l3_x_com*cos(theta_1)*cos(theta_2) - 2*config.l3_y_com*sin(theta_1) + 2*config.l3_z_com*sin(theta_2)*cos(theta_1)) + config.m_hip1*(2*config.l2_x*sin(theta_1) + 2*config.l2_y*cos(theta_1) + 2*config.l3_x_com*sin(theta_1)*cos(theta_2) + 2*config.l3_y_com*cos(theta_1) + 2*config.l3_z_com*sin(theta_1)*sin(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x_com*cos(theta_1)*cos(theta_2) + config.l3_y_com*sin(theta_1) - config.l3_z_com*sin(theta_2)*cos(theta_1)) + config.m_hip2*(config.l2_x_com*sin(theta_1) + config.l2_y_com*cos(theta_1))*(2*config.l2_x_com*cos(theta_1) - 2*config.l2_y_com*sin(theta_1)) + config.m_hip2*(2*config.l2_x_com*sin(theta_1) + 2*config.l2_y_com*cos(theta_1))*(-config.l2_x_com*cos(theta_1) + config.l2_y_com*sin(theta_1)) + config.m_ll*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(2*config.l2_x*cos(theta_1) - 2*config.l2_y*sin(theta_1) + 2*config.l3_x*cos(theta_1)*cos(theta_2) - 2*config.l3_y*sin(theta_1) + 2*config.l3_z*sin(theta_2)*cos(theta_1) + 2*config.l4_x*cos(theta_1)*cos(theta_2) + 2*config.l4_y*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + 2*config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + 2*config.l5_x*cos(theta_1)*cos(theta_2) + 2*config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - 2*config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(2*config.l2_x*sin(theta_1) + 2*config.l2_y*cos(theta_1) + 2*config.l3_x*sin(theta_1)*cos(theta_2) + 2*config.l3_y*cos(theta_1) + 2*config.l3_z*sin(theta_1)*sin(theta_2) + 2*config.l4_x*sin(theta_1)*cos(theta_2) + 2*config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - 2*config.l4_z*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) + 2*config.l5_x*sin(theta_1)*cos(theta_2) - 2*config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - 2*config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ul*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*(2*config.l2_x*cos(theta_1) - 2*config.l2_y*sin(theta_1) + 2*config.l3_x*cos(theta_1)*cos(theta_2) - 2*config.l3_y*sin(theta_1) + 2*config.l3_z*sin(theta_2)*cos(theta_1) + 2*config.l4_x_com*cos(theta_1)*cos(theta_2) + 2*config.l4_y_com*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + 2*config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))) + config.m_ul*(2*config.l2_x*sin(theta_1) + 2*config.l2_y*cos(theta_1) + 2*config.l3_x*sin(theta_1)*cos(theta_2) + 2*config.l3_y*cos(theta_1) + 2*config.l3_z*sin(theta_1)*sin(theta_2) + 2*config.l4_x_com*sin(theta_1)*cos(theta_2) + 2*config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - 2*config.l4_z_com*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))))*thetadot1 + (config.m_foot*(config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l2_x*cos(theta_1) - config.l2_y*sin(theta_1) + config.l3_x*cos(theta_1)*cos(theta_2) - config.l3_y*sin(theta_1) + config.l3_z*sin(theta_2)*cos(theta_1) + config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l5_x*cos(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_x_com*cos(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l4_z*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l2_x*cos(theta_1) - config.l2_y*sin(theta_1) + config.l3_x*cos(theta_1)*cos(theta_2) - config.l3_y*sin(theta_1) + config.l3_z*sin(theta_2)*cos(theta_1) + config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l5_x*cos(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l4_z*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + (-config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2) + (-config.Iyx_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) - config.Izx_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*sin(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyy_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*sin(theta_3)*cos(theta_2) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + (config.Iyz_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*cos(theta_2)*cos(theta_3))*thetadot4 + (config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l4_z*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l2_x*cos(theta_1) - config.l2_y*sin(theta_1) + config.l3_x*cos(theta_1)*cos(theta_2) - config.l3_y*sin(theta_1) + config.l3_z*sin(theta_2)*cos(theta_1) + config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l5_x*cos(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_x_com*cos(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l4_z*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l4_z*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l2_x*cos(theta_1) - config.l2_y*sin(theta_1) + config.l3_x*cos(theta_1)*cos(theta_2) - config.l3_y*sin(theta_1) + config.l3_z*sin(theta_2)*cos(theta_1) + config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l5_x*cos(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l4_z*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l4_z_com*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l2_x*cos(theta_1) - config.l2_y*sin(theta_1) + config.l3_x*cos(theta_1)*cos(theta_2) - config.l3_y*sin(theta_1) + config.l3_z*sin(theta_2)*cos(theta_1) + config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))) + config.m_ul*(-config.l4_y_com*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l4_z_com*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))) + (-config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2) + (-config.Iyx_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) - config.Izx_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*sin(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyy_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*sin(theta_3)*cos(theta_2) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + (config.Iyz_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*cos(theta_2)*cos(theta_3))*thetadot3 + (config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l4_z*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*sin(theta_1) - config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*sin(theta_1) + config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*cos(theta_1) + config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l2_x*cos(theta_1) - config.l2_y*sin(theta_1) + config.l3_x*cos(theta_1)*cos(theta_2) - config.l3_y*sin(theta_1) + config.l3_z*sin(theta_2)*cos(theta_1) + config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l5_x*cos(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_x_com*cos(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*cos(theta_1) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x_com*cos(theta_1)*cos(theta_2) + config.l3_y_com*sin(theta_1) - config.l3_z_com*sin(theta_2)*cos(theta_1))*cos(theta_1) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(config.l2_x*cos(theta_1) - config.l2_y*sin(theta_1) + config.l3_x_com*cos(theta_1)*cos(theta_2) - config.l3_y_com*sin(theta_1) + config.l3_z_com*sin(theta_2)*cos(theta_1))*cos(theta_1) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l4_z*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*sin(theta_1) - config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*sin(theta_1) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*cos(theta_1) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(config.l2_x*cos(theta_1) - config.l2_y*sin(theta_1) + config.l3_x*cos(theta_1)*cos(theta_2) - config.l3_y*sin(theta_1) + config.l3_z*sin(theta_2)*cos(theta_1) + config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l5_x*cos(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*cos(theta_1) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l4_z_com*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)))*sin(theta_1) - config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*sin(theta_1) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*cos(theta_1) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(config.l2_x*cos(theta_1) - config.l2_y*sin(theta_1) + config.l3_x*cos(theta_1)*cos(theta_2) - config.l3_y*sin(theta_1) + config.l3_z*sin(theta_2)*cos(theta_1) + config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*cos(theta_1) + (-config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) - config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*sin(theta_2) + (-config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) - config.Izx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*sin(theta_2) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*sin(theta_3)*cos(theta_2) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + (config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*cos(theta_2)*cos(theta_3))*thetadot2)*thetadot1 - 0.5L*((config.m_foot*(2*config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + 2*config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-2*config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + 2*config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + (-(-config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2))*thetadot5 + (config.m_foot*(config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l2_x*cos(theta_1) - config.l2_y*sin(theta_1) + config.l3_x*cos(theta_1)*cos(theta_2) - config.l3_y*sin(theta_1) + config.l3_z*sin(theta_2)*cos(theta_1) + config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l5_x*cos(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_x_com*cos(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l4_z*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + (((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2))*thetadot1 + (config.m_foot*(config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + (-(-config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2))*thetadot4 + (config.m_foot*(config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l4_z*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + (-(-config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2))*thetadot3 + (config.m_foot*(config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) - config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + (-(-config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) - config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*sin(theta_2) - (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*cos(theta_1)*cos(theta_2) - ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*sin(theta_1)*cos(theta_2) + (-(config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1)*cos(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1)*cos(theta_2))*thetadot2)*thetadot5 - 0.5L*((config.m_foot*(config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) - config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + (-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1) + ((config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1) + (-(config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) - ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1))*thetadot5 + ((-config.Iyx_hip1*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1) + config.Iyy_hip1*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1))*cos(theta_1) + (config.Iyx_hip1*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1) - config.Iyy_hip1*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1))*cos(theta_1) - ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1) + (-(config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1) - ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*sin(theta_1) + (-(config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_1)*cos(theta_2) - (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) - (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*cos(theta_1) + (-config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1)*cos(theta_2) + config.Iyy_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1) - config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_2)*cos(theta_1))*cos(theta_1) + (config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1)*cos(theta_2) - config.Iyy_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_2)*cos(theta_1))*cos(theta_1) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) - (config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*cos(theta_1)*cos(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4)) - (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*sin(theta_1) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*cos(theta_1) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*cos(theta_2) - (config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*cos(theta_1)*cos(theta_2) - (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) - (config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) - (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - (config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*sin(theta_1) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*cos(theta_1))*thetadot2 + (config.m_foot*(config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) - config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_ll*(config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) - config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + (-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*sin(theta_1) + ((config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1) + (-(config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) - ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1) + ((config.Iyx_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*cos(theta_1) + (-(config.Iyx_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Iyy_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) - (config.Iyz_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*sin(theta_1))*thetadot4 + (config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l4_z*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) - config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l4_z*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) - config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l4_z_com*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*sin(theta_1) - config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*sin(theta_1) + config.m_ul*(-config.l4_y_com*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*cos(theta_1) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*sin(theta_1) + ((config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*cos(theta_2) - (-config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*sin(theta_2))*sin(theta_1) - ((config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*sin(theta_2))*sin(theta_1) + (-(config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1) - (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_2)*cos(theta_1))*cos(theta_1) + ((config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_2)*cos(theta_1))*cos(theta_1) + ((config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1) + (-(config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) - ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1) + ((config.Iyx_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*cos(theta_1) + (-(config.Iyx_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Iyy_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) - (config.Iyz_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*sin(theta_1))*thetadot3 + (config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l4_z*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*sin(theta_1) - config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*sin(theta_1) + config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*cos(theta_1) + config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l2_x*cos(theta_1) - config.l2_y*sin(theta_1) + config.l3_x*cos(theta_1)*cos(theta_2) - config.l3_y*sin(theta_1) + config.l3_z*sin(theta_2)*cos(theta_1) + config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l5_x*cos(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_x_com*cos(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*cos(theta_1) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x_com*cos(theta_1)*cos(theta_2) + config.l3_y_com*sin(theta_1) - config.l3_z_com*sin(theta_2)*cos(theta_1))*cos(theta_1) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(config.l2_x*cos(theta_1) - config.l2_y*sin(theta_1) + config.l3_x_com*cos(theta_1)*cos(theta_2) - config.l3_y_com*sin(theta_1) + config.l3_z_com*sin(theta_2)*cos(theta_1))*cos(theta_1) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l4_z*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*sin(theta_1) - config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*sin(theta_1) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*cos(theta_1) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(config.l2_x*cos(theta_1) - config.l2_y*sin(theta_1) + config.l3_x*cos(theta_1)*cos(theta_2) - config.l3_y*sin(theta_1) + config.l3_z*sin(theta_2)*cos(theta_1) + config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l5_x*cos(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*cos(theta_1) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l4_z_com*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)))*sin(theta_1) - config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*sin(theta_1) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*cos(theta_1) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(config.l2_x*cos(theta_1) - config.l2_y*sin(theta_1) + config.l3_x*cos(theta_1)*cos(theta_2) - config.l3_y*sin(theta_1) + config.l3_z*sin(theta_2)*cos(theta_1) + config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*cos(theta_1) + (-config.Izx_hip1*cos(theta_1) + config.Izy_hip1*sin(theta_1))*cos(theta_1) + (config.Izx_hip1*cos(theta_1) - config.Izy_hip1*sin(theta_1))*cos(theta_1) + (-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*cos(theta_1) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*cos(theta_1) + (-((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*sin(theta_1) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) - (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2))*sin(theta_1) - ((-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*sin(theta_1)*cos(theta_2) + (-config.Ixy_ul*sin(theta_2) + config.Izy_ul*cos(theta_2))*cos(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_1)*sin(theta_2))*sin(theta_1) + ((-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*sin(theta_1)*cos(theta_2) - (config.Ixy_ul*sin(theta_2) - config.Izy_ul*cos(theta_2))*cos(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_1)*sin(theta_2))*sin(theta_1) + (-(-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*cos(theta_1)*cos(theta_2) + (-config.Ixy_ul*sin(theta_2) + config.Izy_ul*cos(theta_2))*sin(theta_1) - (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_2)*cos(theta_1))*cos(theta_1) + ((-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*cos(theta_1)*cos(theta_2) - (-config.Ixy_ul*sin(theta_2) + config.Izy_ul*cos(theta_2))*sin(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_2)*cos(theta_1))*cos(theta_1))*thetadot1)*thetadot2 - 0.5L*((config.m_foot*(config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + (-(-config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2))*thetadot5 + (config.m_foot*(config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l2_x*cos(theta_1) - config.l2_y*sin(theta_1) + config.l3_x*cos(theta_1)*cos(theta_2) - config.l3_y*sin(theta_1) + config.l3_z*sin(theta_2)*cos(theta_1) + config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l5_x*cos(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_x_com*cos(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l4_z*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l2_x*cos(theta_1) - config.l2_y*sin(theta_1) + config.l3_x*cos(theta_1)*cos(theta_2) - config.l3_y*sin(theta_1) + config.l3_z*sin(theta_2)*cos(theta_1) + config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l5_x*cos(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l4_z*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + (((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) - (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2))*thetadot1 + (config.m_foot*(2*config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + 2*config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) + 2*config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + 2*config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-2*config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + 2*config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - 2*config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + 2*config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_ll*(2*config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + 2*config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-2*config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + 2*config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1))) + (-(-config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) + (-(-config.Iyx_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) - config.Izx_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*sin(theta_2) - (config.Iyy_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*sin(theta_3)*cos(theta_2) - (config.Iyz_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*cos(theta_2)*cos(theta_3))*sin(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((config.Iyx_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((config.Iyx_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2))*thetadot4 + (config.m_foot*(config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l4_z*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l4_z*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4))) + (-(-config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) + (-(-config.Iyx_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) - config.Izx_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*sin(theta_2) - (config.Iyy_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*sin(theta_3)*cos(theta_2) - (config.Iyz_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*cos(theta_2)*cos(theta_3))*sin(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((config.Iyx_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((config.Iyx_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2))*thetadot3 + (config.m_foot*(config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) - config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_ll*(config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) - config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + (-(-config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) - config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*sin(theta_2) - (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) + (-(-config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) - config.Izx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*sin(theta_2) - (config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*sin(theta_3)*cos(theta_2) - (config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*cos(theta_2)*cos(theta_3))*sin(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*cos(theta_1)*cos(theta_2) - ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*sin(theta_1)*cos(theta_2) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*cos(theta_1)*cos(theta_2) - ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + (-(config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1)*cos(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1)*cos(theta_2) + (-(config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + (config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*cos(theta_1)*cos(theta_2) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*sin(theta_1)*cos(theta_2))*thetadot2)*thetadot4 - 0.5L*((config.m_foot*(config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l4_z*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + (-(-config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2))*thetadot5 + (config.m_foot*(config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l4_z*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l4_z*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4))) + (-(-config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) + (-(-config.Iyx_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) - config.Izx_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*sin(theta_2) - (config.Iyy_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*sin(theta_3)*cos(theta_2) - (config.Iyz_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*cos(theta_2)*cos(theta_3))*sin(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((config.Iyx_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((config.Iyx_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2))*thetadot4 + (config.m_foot*(-2*config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + 2*config.l4_z*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + 2*config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + 2*config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) + 2*config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + 2*config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-2*config.l4_y*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - 2*config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - 2*config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + 2*config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - 2*config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + 2*config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_ll*(-2*config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + 2*config.l4_z*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + 2*config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + 2*config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-2*config.l4_y*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - 2*config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - 2*config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + 2*config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ul*(-2*config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + 2*config.l4_z_com*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)))*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(-2*config.l4_y_com*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - 2*config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))) + (-(-config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) + (-(-config.Iyx_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) - config.Izx_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*sin(theta_2) - (config.Iyy_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*sin(theta_3)*cos(theta_2) - (config.Iyz_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*cos(theta_2)*cos(theta_3))*sin(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + (-(config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (-config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_1) - (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*sin(theta_2))*cos(theta_1)*cos(theta_2) + ((config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*sin(theta_2))*cos(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((config.Iyx_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((config.Iyx_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_ll*((-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2))*thetadot3 + (config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l4_z*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l2_x*cos(theta_1) - config.l2_y*sin(theta_1) + config.l3_x*cos(theta_1)*cos(theta_2) - config.l3_y*sin(theta_1) + config.l3_z*sin(theta_2)*cos(theta_1) + config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l5_x*cos(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_x_com*cos(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l4_z*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l4_z*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l2_x*cos(theta_1) - config.l2_y*sin(theta_1) + config.l3_x*cos(theta_1)*cos(theta_2) - config.l3_y*sin(theta_1) + config.l3_z*sin(theta_2)*cos(theta_1) + config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l5_x*cos(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l4_z*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l4_z_com*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l2_x*cos(theta_1) - config.l2_y*sin(theta_1) + config.l3_x*cos(theta_1)*cos(theta_2) - config.l3_y*sin(theta_1) + config.l3_z*sin(theta_2)*cos(theta_1) + config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))) + config.m_ul*(-config.l4_y_com*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l4_z_com*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + (((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) - (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + (-(-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*sin(theta_1)*cos(theta_2) + (config.Ixy_ul*sin(theta_2) - config.Izy_ul*cos(theta_2))*cos(theta_1) - (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_1)*sin(theta_2))*cos(theta_1)*cos(theta_2) + ((-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*sin(theta_1)*cos(theta_2) + (-config.Ixy_ul*sin(theta_2) + config.Izy_ul*cos(theta_2))*cos(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_1)*sin(theta_2))*cos(theta_1)*cos(theta_2))*thetadot1 + (config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l4_z*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) - config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l4_z*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_1) - sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) - config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + config.l4_z_com*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*sin(theta_1) - config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*sin(theta_1) + config.m_ul*(-config.l4_y_com*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*cos(theta_1) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-(-config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) - config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*sin(theta_2) - (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) + (-(-config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) - config.Izx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*sin(theta_2) - (config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*sin(theta_3)*cos(theta_2) - (config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*cos(theta_2)*cos(theta_3))*sin(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*cos(theta_1)*cos(theta_2) - ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*sin(theta_1)*cos(theta_2) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*cos(theta_1)*cos(theta_2) - ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + (-config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1)*cos(theta_2) - config.Iyy_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1) - config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1)*sin(theta_2))*cos(theta_1)*cos(theta_2) + (config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1)*cos(theta_2) + config.Iyy_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1)*sin(theta_2))*cos(theta_1)*cos(theta_2) + (-(config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1)*cos(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1) + (-(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1)*cos(theta_2) + (-(config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + (config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*cos(theta_1)*cos(theta_2) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*sin(theta_1)*cos(theta_2))*thetadot2)*thetadot3 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l2_x*thetadot1*cos(theta_1) - config.l2_y*thetadot1*sin(theta_1) + config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - config.l3_y*thetadot1*sin(theta_1) + config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l4_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l4_y*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l4_z*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l5_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l5_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l6_x_com*thetadot1*cos(theta_1)*cos(theta_2) - config.l6_x_com*thetadot2*sin(theta_1)*sin(theta_2) + config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)) - config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*thetadot1*sin(theta_1) + config.l2_y*thetadot1*cos(theta_1) + config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l3_y*thetadot1*cos(theta_1) + config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + config.l4_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l4_y*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l5_x*thetadot2*sin(theta_2)*cos(theta_1) - config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l6_x_com*thetadot1*sin(theta_1)*cos(theta_2) + config.l6_x_com*thetadot2*sin(theta_2)*cos(theta_1) - config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) - config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))) + config.m_foot*(config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*thetadot1*sin(theta_1)*cos(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*thetadot2*sin(theta_2)*cos(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2))*thetadot2*cos(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*thetadot2*cos(theta_2) + config.Iyz_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*thetadot2*cos(theta_2) + config.Iyy_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) - (config.Ixx_foot*sin(theta_2) - config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot2*cos(theta_2) - (-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Ixx_foot*thetadot2*cos(theta_2) - config.Iyx_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - config.Izx_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_2))*sin(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*thetadot2*cos(theta_2) + config.Iyy_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*thetadot2*cos(theta_2) + config.Iyz_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot1*sin(theta_1)*cos(theta_2) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot2*sin(theta_2)*cos(theta_1) + (-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (-config.Ixx_foot*thetadot2*cos(theta_2) + config.Iyx_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*thetadot2*cos(theta_2) + config.Iyz_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*thetadot2*cos(theta_2) + config.Iyy_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot1*cos(theta_1)*cos(theta_2) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot2*sin(theta_1)*sin(theta_2) + (-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (-config.Ixx_foot*thetadot2*cos(theta_2) + config.Iyx_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2))*thetadot5 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l2_x*thetadot1*cos(theta_1) - config.l2_y*thetadot1*sin(theta_1) + config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - config.l3_y*thetadot1*sin(theta_1) + config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l4_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l4_y*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l4_z*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l5_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l5_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l6_x_com*thetadot1*cos(theta_1)*cos(theta_2) - config.l6_x_com*thetadot2*sin(theta_1)*sin(theta_2) + config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)) - config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*thetadot1*sin(theta_1) + config.l2_y*thetadot1*cos(theta_1) + config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l3_y*thetadot1*cos(theta_1) + config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + config.l4_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l4_y*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l5_x*thetadot2*sin(theta_2)*cos(theta_1) - config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l6_x_com*thetadot1*sin(theta_1)*cos(theta_2) + config.l6_x_com*thetadot2*sin(theta_2)*cos(theta_1) - config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) - config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))) + config.m_foot*(config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) + config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l2_x*thetadot1*cos(theta_1) - config.l2_y*thetadot1*sin(theta_1) + config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - config.l3_y*thetadot1*sin(theta_1) + config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l4_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l4_y*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l4_z*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l5_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l5_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*thetadot1*sin(theta_1) + config.l2_y*thetadot1*cos(theta_1) + config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l3_y*thetadot1*cos(theta_1) + config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + config.l4_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l4_y*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l5_x*thetadot2*sin(theta_2)*cos(theta_1) - config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))) + config.m_ll*(config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*thetadot1*sin(theta_1)*cos(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*thetadot2*sin(theta_2)*cos(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2))*thetadot2*cos(theta_2) + ((-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_2) - (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*sin(theta_3)*cos(theta_2) - (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*thetadot2*cos(theta_2) + config.Iyz_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*thetadot2*cos(theta_2) + config.Iyy_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) - (config.Ixx_foot*sin(theta_2) - config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot2*cos(theta_2) - (-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Ixx_foot*thetadot2*cos(theta_2) - config.Iyx_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - config.Izx_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_2))*sin(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*thetadot2*cos(theta_2) + config.Iyy_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*thetadot2*cos(theta_2) + config.Iyz_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot1*sin(theta_1)*cos(theta_2) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot2*sin(theta_2)*cos(theta_1) + (-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (-config.Ixx_foot*thetadot2*cos(theta_2) + config.Iyx_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*thetadot2*cos(theta_2) + config.Iyz_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*thetadot2*cos(theta_2) + config.Iyy_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot1*cos(theta_1)*cos(theta_2) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot2*sin(theta_1)*sin(theta_2) + (-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (-config.Ixx_foot*thetadot2*cos(theta_2) + config.Iyx_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(-config.Ixz_ll*thetadot2*cos(theta_2) - config.Iyz_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyz_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izz_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izz_ll*thetadot3*sin(theta_3)*cos(theta_2)) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(-config.Ixy_ll*thetadot2*cos(theta_2) - config.Iyy_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyy_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izy_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izy_ll*thetadot3*sin(theta_3)*cos(theta_2)) - (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*(-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) + (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + (-config.Ixx_ll*thetadot2*cos(theta_2) - config.Iyx_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyx_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izx_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izx_ll*thetadot3*sin(theta_3)*cos(theta_2))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*thetadot2*cos(theta_2) - config.Iyy_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyy_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izy_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izy_ll*thetadot3*sin(theta_3)*cos(theta_2)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*thetadot2*cos(theta_2) - config.Iyz_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyz_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izz_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izz_ll*thetadot3*sin(theta_3)*cos(theta_2)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + (-config.Ixx_ll*thetadot2*cos(theta_2) - config.Iyx_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyx_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izx_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izx_ll*thetadot3*sin(theta_3)*cos(theta_2))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + (-(config.Ixx_ll*sin(theta_2) - config.Iyx_ll*sin(theta_3)*cos(theta_2) - config.Izx_ll*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*thetadot2*sin(theta_2)*sin(theta_3) - (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*thetadot3*cos(theta_2)*cos(theta_3) + (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_3) + (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*thetadot3*sin(theta_3)*cos(theta_2) - (config.Ixx_ll*thetadot2*cos(theta_2) + config.Iyx_ll*thetadot2*sin(theta_2)*sin(theta_3) - config.Iyx_ll*thetadot3*cos(theta_2)*cos(theta_3) + config.Izx_ll*thetadot2*sin(theta_2)*cos(theta_3) + config.Izx_ll*thetadot3*sin(theta_3)*cos(theta_2))*sin(theta_2) - (-config.Ixy_ll*thetadot2*cos(theta_2) - config.Iyy_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyy_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izy_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izy_ll*thetadot3*sin(theta_3)*cos(theta_2))*sin(theta_3)*cos(theta_2) - (-config.Ixz_ll*thetadot2*cos(theta_2) - config.Iyz_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyz_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izz_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izz_ll*thetadot3*sin(theta_3)*cos(theta_2))*cos(theta_2)*cos(theta_3))*sin(theta_2))*thetadot4 + (config.m_foot*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(2*config.l2_x*thetadot1*cos(theta_1) - 2*config.l2_y*thetadot1*sin(theta_1) + 2*config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - 2*config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - 2*config.l3_y*thetadot1*sin(theta_1) + 2*config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + 2*config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + 2*config.l4_x*thetadot1*cos(theta_1)*cos(theta_2) - 2*config.l4_x*thetadot2*sin(theta_1)*sin(theta_2) + 2*config.l4_y*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + 2*config.l4_z*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + 2*config.l5_x*thetadot1*cos(theta_1)*cos(theta_2) - 2*config.l5_x*thetadot2*sin(theta_1)*sin(theta_2) + 2*config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - 2*config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + 2*config.l6_x_com*thetadot1*cos(theta_1)*cos(theta_2) - 2*config.l6_x_com*thetadot2*sin(theta_1)*sin(theta_2) + 2*config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)) - 2*config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))) + config.m_foot*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(2*config.l2_x*thetadot1*sin(theta_1) + 2*config.l2_y*thetadot1*cos(theta_1) + 2*config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + 2*config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + 2*config.l3_y*thetadot1*cos(theta_1) + 2*config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - 2*config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + 2*config.l4_x*thetadot1*sin(theta_1)*cos(theta_2) + 2*config.l4_x*thetadot2*sin(theta_2)*cos(theta_1) + 2*config.l4_y*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - 2*config.l4_z*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + 2*config.l5_x*thetadot1*sin(theta_1)*cos(theta_2) + 2*config.l5_x*thetadot2*sin(theta_2)*cos(theta_1) - 2*config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - 2*config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + 2*config.l6_x_com*thetadot1*sin(theta_1)*cos(theta_2) + 2*config.l6_x_com*thetadot2*sin(theta_2)*cos(theta_1) - 2*config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) - 2*config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))) + config.m_hip1*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x_com*sin(theta_1)*cos(theta_2) + config.l3_y_com*cos(theta_1) + config.l3_z_com*sin(theta_1)*sin(theta_2))*(2*config.l2_x*thetadot1*cos(theta_1) - 2*config.l2_y*thetadot1*sin(theta_1) + 2*config.l3_x_com*thetadot1*cos(theta_1)*cos(theta_2) - 2*config.l3_x_com*thetadot2*sin(theta_1)*sin(theta_2) - 2*config.l3_y_com*thetadot1*sin(theta_1) + 2*config.l3_z_com*thetadot1*sin(theta_2)*cos(theta_1) + 2*config.l3_z_com*thetadot2*sin(theta_1)*cos(theta_2)) + config.m_hip1*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x_com*cos(theta_1)*cos(theta_2) + config.l3_y_com*sin(theta_1) - config.l3_z_com*sin(theta_2)*cos(theta_1))*(2*config.l2_x*thetadot1*sin(theta_1) + 2*config.l2_y*thetadot1*cos(theta_1) + 2*config.l3_x_com*thetadot1*sin(theta_1)*cos(theta_2) + 2*config.l3_x_com*thetadot2*sin(theta_2)*cos(theta_1) + 2*config.l3_y_com*thetadot1*cos(theta_1) + 2*config.l3_z_com*thetadot1*sin(theta_1)*sin(theta_2) - 2*config.l3_z_com*thetadot2*cos(theta_1)*cos(theta_2)) + config.m_hip2*(config.l2_x_com*sin(theta_1) + config.l2_y_com*cos(theta_1))*(2*config.l2_x_com*thetadot1*cos(theta_1) - 2*config.l2_y_com*thetadot1*sin(theta_1)) + config.m_hip2*(-config.l2_x_com*cos(theta_1) + config.l2_y_com*sin(theta_1))*(2*config.l2_x_com*thetadot1*sin(theta_1) + 2*config.l2_y_com*thetadot1*cos(theta_1)) + config.m_ll*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(2*config.l2_x*thetadot1*cos(theta_1) - 2*config.l2_y*thetadot1*sin(theta_1) + 2*config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - 2*config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - 2*config.l3_y*thetadot1*sin(theta_1) + 2*config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + 2*config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + 2*config.l4_x*thetadot1*cos(theta_1)*cos(theta_2) - 2*config.l4_x*thetadot2*sin(theta_1)*sin(theta_2) + 2*config.l4_y*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + 2*config.l4_z*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + 2*config.l5_x*thetadot1*cos(theta_1)*cos(theta_2) - 2*config.l5_x*thetadot2*sin(theta_1)*sin(theta_2) + 2*config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - 2*config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2))) + config.m_ll*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(2*config.l2_x*thetadot1*sin(theta_1) + 2*config.l2_y*thetadot1*cos(theta_1) + 2*config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + 2*config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + 2*config.l3_y*thetadot1*cos(theta_1) + 2*config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - 2*config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + 2*config.l4_x*thetadot1*sin(theta_1)*cos(theta_2) + 2*config.l4_x*thetadot2*sin(theta_2)*cos(theta_1) + 2*config.l4_y*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - 2*config.l4_z*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + 2*config.l5_x*thetadot1*sin(theta_1)*cos(theta_2) + 2*config.l5_x*thetadot2*sin(theta_2)*cos(theta_1) - 2*config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - 2*config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))) + config.m_ul*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*(2*config.l2_x*thetadot1*cos(theta_1) - 2*config.l2_y*thetadot1*sin(theta_1) + 2*config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - 2*config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - 2*config.l3_y*thetadot1*sin(theta_1) + 2*config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + 2*config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + 2*config.l4_x_com*thetadot1*cos(theta_1)*cos(theta_2) - 2*config.l4_x_com*thetadot2*sin(theta_1)*sin(theta_2) + 2*config.l4_y_com*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + 2*config.l4_z_com*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))) + config.m_ul*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*(2*config.l2_x*thetadot1*sin(theta_1) + 2*config.l2_y*thetadot1*cos(theta_1) + 2*config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + 2*config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + 2*config.l3_y*thetadot1*cos(theta_1) + 2*config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - 2*config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + 2*config.l4_x_com*thetadot1*sin(theta_1)*cos(theta_2) + 2*config.l4_x_com*thetadot2*sin(theta_2)*cos(theta_1) + 2*config.l4_y_com*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - 2*config.l4_z_com*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))) + (config.Ixx_ul*sin(theta_2) - config.Izx_ul*cos(theta_2))*thetadot2*cos(theta_2) - (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*thetadot2*sin(theta_2) + (config.Ixx_ul*thetadot2*cos(theta_2) + config.Izx_ul*thetadot2*sin(theta_2))*sin(theta_2) + (-config.Ixz_ul*thetadot2*cos(theta_2) - config.Izz_ul*thetadot2*sin(theta_2))*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*thetadot2*cos(theta_2) + config.Iyz_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*thetadot2*cos(theta_2) + config.Iyy_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) + (config.Ixx_foot*sin(theta_2) - config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot2*cos(theta_2) + (config.Ixx_ll*sin(theta_2) - config.Iyx_ll*sin(theta_3)*cos(theta_2) - config.Izx_ll*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + (-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*thetadot2*sin(theta_2)*sin(theta_3) + (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*thetadot3*cos(theta_2)*cos(theta_3) + (-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)) - (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_3) - (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*thetadot3*sin(theta_3)*cos(theta_2) + (config.Ixx_foot*thetadot2*cos(theta_2) - config.Iyx_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - config.Izx_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_2) + (config.Ixx_ll*thetadot2*cos(theta_2) + config.Iyx_ll*thetadot2*sin(theta_2)*sin(theta_3) - config.Iyx_ll*thetadot3*cos(theta_2)*cos(theta_3) + config.Izx_ll*thetadot2*sin(theta_2)*cos(theta_3) + config.Izx_ll*thetadot3*sin(theta_3)*cos(theta_2))*sin(theta_2) + (-config.Ixy_ll*thetadot2*cos(theta_2) - config.Iyy_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyy_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izy_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izy_ll*thetadot3*sin(theta_3)*cos(theta_2))*sin(theta_3)*cos(theta_2) + (-config.Ixz_ll*thetadot2*cos(theta_2) - config.Iyz_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyz_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izz_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izz_ll*thetadot3*sin(theta_3)*cos(theta_2))*cos(theta_2)*cos(theta_3))*thetadot1 + (config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l2_x*thetadot1*cos(theta_1) - config.l2_y*thetadot1*sin(theta_1) + config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - config.l3_y*thetadot1*sin(theta_1) + config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l4_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l4_y*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l4_z*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l5_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l5_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l6_x_com*thetadot1*cos(theta_1)*cos(theta_2) - config.l6_x_com*thetadot2*sin(theta_1)*sin(theta_2) + config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)) - config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*thetadot1*sin(theta_1) + config.l2_y*thetadot1*cos(theta_1) + config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l3_y*thetadot1*cos(theta_1) + config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + config.l4_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l4_y*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l5_x*thetadot2*sin(theta_2)*cos(theta_1) - config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l6_x_com*thetadot1*sin(theta_1)*cos(theta_2) + config.l6_x_com*thetadot2*sin(theta_2)*cos(theta_1) - config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) - config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))) + config.m_foot*(-config.l4_y*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l4_z*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) + config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l4_y*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l2_x*thetadot1*cos(theta_1) - config.l2_y*thetadot1*sin(theta_1) + config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - config.l3_y*thetadot1*sin(theta_1) + config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l4_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l4_y*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l4_z*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l5_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l5_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2))) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*thetadot1*sin(theta_1) + config.l2_y*thetadot1*cos(theta_1) + config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l3_y*thetadot1*cos(theta_1) + config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + config.l4_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l4_y*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l5_x*thetadot2*sin(theta_2)*cos(theta_1) - config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))) + config.m_ll*(-config.l4_y*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l4_z*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l4_y*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l2_x*thetadot1*cos(theta_1) - config.l2_y*thetadot1*sin(theta_1) + config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - config.l3_y*thetadot1*sin(theta_1) + config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + config.l4_x_com*thetadot1*cos(theta_1)*cos(theta_2) - config.l4_x_com*thetadot2*sin(theta_1)*sin(theta_2) + config.l4_y_com*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l4_z_com*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(config.l2_x*thetadot1*sin(theta_1) + config.l2_y*thetadot1*cos(theta_1) + config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l3_y*thetadot1*cos(theta_1) + config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + config.l4_x_com*thetadot1*sin(theta_1)*cos(theta_2) + config.l4_x_com*thetadot2*sin(theta_2)*cos(theta_1) + config.l4_y_com*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))) + config.m_ul*(-config.l4_y_com*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l4_z_com*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))) + config.m_ul*(-config.l4_y_com*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))) + ((-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*sin(theta_2) - (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*cos(theta_2))*thetadot2*cos(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*thetadot1*sin(theta_1)*cos(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*thetadot2*sin(theta_2)*cos(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2))*thetadot2*cos(theta_2) + ((-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_2) - (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*sin(theta_3)*cos(theta_2) - (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*sin(theta_1)*cos(theta_2) + (-config.Ixy_ul*sin(theta_2) + config.Izy_ul*cos(theta_2))*cos(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_1)*sin(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - ((-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*sin(theta_1)*cos(theta_2) + (-config.Ixy_ul*sin(theta_2) + config.Izy_ul*cos(theta_2))*cos(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_1)*sin(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) - ((-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*cos(theta_1)*cos(theta_2) - (-config.Ixy_ul*sin(theta_2) + config.Izy_ul*cos(theta_2))*sin(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_2)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - ((-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*cos(theta_1)*cos(theta_2) - (-config.Ixy_ul*sin(theta_2) + config.Izy_ul*cos(theta_2))*sin(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_2)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (-(config.Ixx_ul*sin(theta_2) - config.Izx_ul*cos(theta_2))*thetadot2*cos(theta_2) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*thetadot2*sin(theta_2) - (config.Ixx_ul*thetadot2*cos(theta_2) + config.Izx_ul*thetadot2*sin(theta_2))*sin(theta_2) - (-config.Ixz_ul*thetadot2*cos(theta_2) - config.Izz_ul*thetadot2*sin(theta_2))*cos(theta_2))*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*thetadot2*cos(theta_2) + config.Iyz_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*thetadot2*cos(theta_2) + config.Iyy_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) - (config.Ixx_foot*sin(theta_2) - config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot2*cos(theta_2) - (-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Ixx_foot*thetadot2*cos(theta_2) - config.Iyx_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - config.Izx_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_2))*sin(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*thetadot2*cos(theta_2) + config.Iyy_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*thetadot2*cos(theta_2) + config.Iyz_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot1*sin(theta_1)*cos(theta_2) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot2*sin(theta_2)*cos(theta_1) + (-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (-config.Ixx_foot*thetadot2*cos(theta_2) + config.Iyx_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*thetadot2*cos(theta_2) + config.Iyz_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*thetadot2*cos(theta_2) + config.Iyy_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot1*cos(theta_1)*cos(theta_2) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot2*sin(theta_1)*sin(theta_2) + (-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (-config.Ixx_foot*thetadot2*cos(theta_2) + config.Iyx_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(-config.Ixz_ll*thetadot2*cos(theta_2) - config.Iyz_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyz_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izz_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izz_ll*thetadot3*sin(theta_3)*cos(theta_2)) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(-config.Ixy_ll*thetadot2*cos(theta_2) - config.Iyy_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyy_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izy_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izy_ll*thetadot3*sin(theta_3)*cos(theta_2)) - (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*(-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) + (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + (-config.Ixx_ll*thetadot2*cos(theta_2) - config.Iyx_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyx_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izx_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izx_ll*thetadot3*sin(theta_3)*cos(theta_2))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*thetadot2*cos(theta_2) - config.Iyy_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyy_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izy_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izy_ll*thetadot3*sin(theta_3)*cos(theta_2)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*thetadot2*cos(theta_2) - config.Iyz_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyz_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izz_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izz_ll*thetadot3*sin(theta_3)*cos(theta_2)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + (-config.Ixx_ll*thetadot2*cos(theta_2) - config.Iyx_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyx_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izx_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izx_ll*thetadot3*sin(theta_3)*cos(theta_2))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + (-(config.Ixx_ll*sin(theta_2) - config.Iyx_ll*sin(theta_3)*cos(theta_2) - config.Izx_ll*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*thetadot2*sin(theta_2)*sin(theta_3) - (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*thetadot3*cos(theta_2)*cos(theta_3) + (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_3) + (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*thetadot3*sin(theta_3)*cos(theta_2) - (config.Ixx_ll*thetadot2*cos(theta_2) + config.Iyx_ll*thetadot2*sin(theta_2)*sin(theta_3) - config.Iyx_ll*thetadot3*cos(theta_2)*cos(theta_3) + config.Izx_ll*thetadot2*sin(theta_2)*cos(theta_3) + config.Izx_ll*thetadot3*sin(theta_3)*cos(theta_2))*sin(theta_2) - (-config.Ixy_ll*thetadot2*cos(theta_2) - config.Iyy_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyy_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izy_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izy_ll*thetadot3*sin(theta_3)*cos(theta_2))*sin(theta_3)*cos(theta_2) - (-config.Ixz_ll*thetadot2*cos(theta_2) - config.Iyz_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyz_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izz_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izz_ll*thetadot3*sin(theta_3)*cos(theta_2))*cos(theta_2)*cos(theta_3))*sin(theta_2) + (-(-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*thetadot1*sin(theta_1)*cos(theta_2) - (-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*thetadot2*sin(theta_2)*cos(theta_1) + (config.Ixy_ul*sin(theta_2) - config.Izy_ul*cos(theta_2))*thetadot1*cos(theta_1) - (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*thetadot1*sin(theta_1)*sin(theta_2) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*thetadot2*cos(theta_1)*cos(theta_2) + (-config.Ixx_ul*thetadot2*cos(theta_2) - config.Izx_ul*thetadot2*sin(theta_2))*cos(theta_1)*cos(theta_2) + (config.Ixy_ul*thetadot2*cos(theta_2) + config.Izy_ul*thetadot2*sin(theta_2))*sin(theta_1) + (-config.Ixz_ul*thetadot2*cos(theta_2) - config.Izz_ul*thetadot2*sin(theta_2))*sin(theta_2)*cos(theta_1))*cos(theta_1)*cos(theta_2) + ((-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - (-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) - (-config.Ixy_ul*sin(theta_2) + config.Izy_ul*cos(theta_2))*thetadot1*sin(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*thetadot1*sin(theta_2)*cos(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*thetadot2*sin(theta_1)*cos(theta_2) + (-config.Ixx_ul*thetadot2*cos(theta_2) - config.Izx_ul*thetadot2*sin(theta_2))*sin(theta_1)*cos(theta_2) + (-config.Ixy_ul*thetadot2*cos(theta_2) - config.Izy_ul*thetadot2*sin(theta_2))*cos(theta_1) + (-config.Ixz_ul*thetadot2*cos(theta_2) - config.Izz_ul*thetadot2*sin(theta_2))*sin(theta_1)*sin(theta_2))*sin(theta_1)*cos(theta_2))*thetadot3 + (-config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*thetadot1*sin(theta_1) + config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*thetadot1*cos(theta_1) + config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l2_x*thetadot1*sin(theta_1) + config.l2_y*thetadot1*cos(theta_1) + config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l3_y*thetadot1*cos(theta_1) + config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + config.l4_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l4_y*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l5_x*thetadot2*sin(theta_2)*cos(theta_1) - config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l6_x_com*thetadot1*sin(theta_1)*cos(theta_2) + config.l6_x_com*thetadot2*sin(theta_2)*cos(theta_1) - config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) - config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)))*sin(theta_1) + config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l2_x*thetadot1*cos(theta_1) - config.l2_y*thetadot1*sin(theta_1) + config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - config.l3_y*thetadot1*sin(theta_1) + config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l4_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l4_y*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l4_z*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l5_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l5_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l6_x_com*thetadot1*cos(theta_1)*cos(theta_2) - config.l6_x_com*thetadot2*sin(theta_1)*sin(theta_2) + config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)) - config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)))*cos(theta_1) + config.m_foot*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*thetadot2*cos(theta_2) - config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*thetadot2*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_1) + config.m_foot*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*thetadot2*cos(theta_2) - config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*thetadot2*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*sin(theta_1) - config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x_com*sin(theta_1)*cos(theta_2) + config.l3_y_com*cos(theta_1) + config.l3_z_com*sin(theta_1)*sin(theta_2))*thetadot1*sin(theta_1) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x_com*cos(theta_1)*cos(theta_2) + config.l3_y_com*sin(theta_1) - config.l3_z_com*sin(theta_2)*cos(theta_1))*thetadot1*cos(theta_1) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(config.l2_x*thetadot1*sin(theta_1) + config.l2_y*thetadot1*cos(theta_1) + config.l3_x_com*thetadot1*sin(theta_1)*cos(theta_2) + config.l3_x_com*thetadot2*sin(theta_2)*cos(theta_1) + config.l3_y_com*thetadot1*cos(theta_1) + config.l3_z_com*thetadot1*sin(theta_1)*sin(theta_2) - config.l3_z_com*thetadot2*cos(theta_1)*cos(theta_2))*sin(theta_1) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(config.l2_x*thetadot1*cos(theta_1) - config.l2_y*thetadot1*sin(theta_1) + config.l3_x_com*thetadot1*cos(theta_1)*cos(theta_2) - config.l3_x_com*thetadot2*sin(theta_1)*sin(theta_2) - config.l3_y_com*thetadot1*sin(theta_1) + config.l3_z_com*thetadot1*sin(theta_2)*cos(theta_1) + config.l3_z_com*thetadot2*sin(theta_1)*cos(theta_2))*cos(theta_1) + config.m_hip1*(config.l3_x_com*thetadot2*cos(theta_2) + config.l3_z_com*thetadot2*sin(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x_com*sin(theta_1)*cos(theta_2) + config.l3_y_com*cos(theta_1) + config.l3_z_com*sin(theta_1)*sin(theta_2))*cos(theta_1) + config.m_hip1*(config.l3_x_com*thetadot2*cos(theta_2) + config.l3_z_com*thetadot2*sin(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x_com*cos(theta_1)*cos(theta_2) + config.l3_y_com*sin(theta_1) - config.l3_z_com*sin(theta_2)*cos(theta_1))*sin(theta_1) - config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*thetadot1*sin(theta_1) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*thetadot1*cos(theta_1) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(config.l2_x*thetadot1*sin(theta_1) + config.l2_y*thetadot1*cos(theta_1) + config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l3_y*thetadot1*cos(theta_1) + config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + config.l4_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l4_y*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l5_x*thetadot2*sin(theta_2)*cos(theta_1) - config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)))*sin(theta_1) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(config.l2_x*thetadot1*cos(theta_1) - config.l2_y*thetadot1*sin(theta_1) + config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - config.l3_y*thetadot1*sin(theta_1) + config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l4_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l4_y*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l4_z*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l5_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l5_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)))*cos(theta_1) + config.m_ll*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_1) + config.m_ll*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4))*sin(theta_1) - config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*thetadot1*sin(theta_1) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*thetadot1*cos(theta_1) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(config.l2_x*thetadot1*sin(theta_1) + config.l2_y*thetadot1*cos(theta_1) + config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l3_y*thetadot1*cos(theta_1) + config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + config.l4_x_com*thetadot1*sin(theta_1)*cos(theta_2) + config.l4_x_com*thetadot2*sin(theta_2)*cos(theta_1) + config.l4_y_com*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)))*sin(theta_1) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(config.l2_x*thetadot1*cos(theta_1) - config.l2_y*thetadot1*sin(theta_1) + config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - config.l3_y*thetadot1*sin(theta_1) + config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + config.l4_x_com*thetadot1*cos(theta_1)*cos(theta_2) - config.l4_x_com*thetadot2*sin(theta_1)*sin(theta_2) + config.l4_y_com*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l4_z_com*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)))*cos(theta_1) + config.m_ul*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x_com*thetadot2*cos(theta_2) + config.l4_y_com*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y_com*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z_com*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z_com*thetadot3*sin(theta_3)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*cos(theta_1) + config.m_ul*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x_com*thetadot2*cos(theta_2) + config.l4_y_com*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y_com*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z_com*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z_com*thetadot3*sin(theta_3)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*sin(theta_1) - (config.Izx_hip1*sin(theta_1) + config.Izy_hip1*cos(theta_1))*thetadot1*sin(theta_1) + (-config.Izx_hip1*cos(theta_1) + config.Izy_hip1*sin(theta_1))*thetadot1*cos(theta_1) + (config.Izx_hip1*thetadot1*sin(theta_1) + config.Izy_hip1*thetadot1*cos(theta_1))*sin(theta_1) + (config.Izx_hip1*thetadot1*cos(theta_1) - config.Izy_hip1*thetadot1*sin(theta_1))*cos(theta_1) + (-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*thetadot1*cos(theta_1) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) - (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2))*thetadot1*sin(theta_1) - ((-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*sin(theta_1)*cos(theta_2) + (-config.Ixy_ul*sin(theta_2) + config.Izy_ul*cos(theta_2))*cos(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_1)*sin(theta_2))*thetadot1*sin(theta_1) + (-(-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*cos(theta_1)*cos(theta_2) + (-config.Ixy_ul*sin(theta_2) + config.Izy_ul*cos(theta_2))*sin(theta_1) - (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_2)*cos(theta_1))*thetadot1*cos(theta_1) + (-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*thetadot2*cos(theta_2) + config.Iyy_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*thetadot2*cos(theta_2) + config.Iyz_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot1*sin(theta_1)*cos(theta_2) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot2*sin(theta_2)*cos(theta_1) - (-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) - (-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (-config.Ixx_foot*thetadot2*cos(theta_2) + config.Iyx_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*sin(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*thetadot2*cos(theta_2) + config.Iyz_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*thetadot2*cos(theta_2) + config.Iyy_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot1*cos(theta_1)*cos(theta_2) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot2*sin(theta_1)*sin(theta_2) + (-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (-config.Ixx_foot*thetadot2*cos(theta_2) + config.Iyx_foot*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*cos(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(-config.Ixz_ll*thetadot2*cos(theta_2) - config.Iyz_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyz_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izz_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izz_ll*thetadot3*sin(theta_3)*cos(theta_2)) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(-config.Ixy_ll*thetadot2*cos(theta_2) - config.Iyy_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyy_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izy_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izy_ll*thetadot3*sin(theta_3)*cos(theta_2)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) - (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*(-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - (-config.Ixx_ll*thetadot2*cos(theta_2) - config.Iyx_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyx_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izx_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izx_ll*thetadot3*sin(theta_3)*cos(theta_2))*cos(theta_1)*cos(theta_2))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*thetadot2*cos(theta_2) - config.Iyy_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyy_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izy_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izy_ll*thetadot3*sin(theta_3)*cos(theta_2)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*thetadot2*cos(theta_2) - config.Iyz_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyz_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izz_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izz_ll*thetadot3*sin(theta_3)*cos(theta_2)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + (-config.Ixx_ll*thetadot2*cos(theta_2) - config.Iyx_ll*thetadot2*sin(theta_2)*sin(theta_3) + config.Iyx_ll*thetadot3*cos(theta_2)*cos(theta_3) - config.Izx_ll*thetadot2*sin(theta_2)*cos(theta_3) - config.Izx_ll*thetadot3*sin(theta_3)*cos(theta_2))*sin(theta_1)*cos(theta_2))*cos(theta_1) + ((-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*thetadot1*sin(theta_1)*cos(theta_2) + (-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*thetadot2*sin(theta_2)*cos(theta_1) - (config.Ixy_ul*sin(theta_2) - config.Izy_ul*cos(theta_2))*thetadot1*cos(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*thetadot1*sin(theta_1)*sin(theta_2) - (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*thetadot2*cos(theta_1)*cos(theta_2) - (-config.Ixx_ul*thetadot2*cos(theta_2) - config.Izx_ul*thetadot2*sin(theta_2))*cos(theta_1)*cos(theta_2) - (config.Ixy_ul*thetadot2*cos(theta_2) + config.Izy_ul*thetadot2*sin(theta_2))*sin(theta_1) - (-config.Ixz_ul*thetadot2*cos(theta_2) - config.Izz_ul*thetadot2*sin(theta_2))*sin(theta_2)*cos(theta_1))*sin(theta_1) + ((-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - (-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) - (-config.Ixy_ul*sin(theta_2) + config.Izy_ul*cos(theta_2))*thetadot1*sin(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*thetadot1*sin(theta_2)*cos(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*thetadot2*sin(theta_1)*cos(theta_2) + (-config.Ixx_ul*thetadot2*cos(theta_2) - config.Izx_ul*thetadot2*sin(theta_2))*sin(theta_1)*cos(theta_2) + (-config.Ixy_ul*thetadot2*cos(theta_2) - config.Izy_ul*thetadot2*sin(theta_2))*cos(theta_1) + (-config.Ixz_ul*thetadot2*cos(theta_2) - config.Izz_ul*thetadot2*sin(theta_2))*sin(theta_1)*sin(theta_2))*cos(theta_1))*thetadot2;

    double temp_2 = -0.5L*((config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*sin(theta_1) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l3_x*sin(theta_2) + config.l3_z*cos(theta_2) - config.l4_x*sin(theta_2) + config.l4_y*sin(theta_3)*cos(theta_2) + config.l4_z*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x_com*sin(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_2) - config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*sin(theta_2) + config.m_foot*(-config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + (-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2)*cos(theta_1) - (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1))*thetadot5 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_1) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*sin(theta_1) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l3_x*sin(theta_2) + config.l3_z*cos(theta_2) - config.l4_x*sin(theta_2) + config.l4_y*sin(theta_3)*cos(theta_2) + config.l4_z*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x_com*sin(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_2) - config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*sin(theta_2) + config.m_foot*(-config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) - config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_1) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*sin(theta_1) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(-config.l3_x*sin(theta_2) + config.l3_z*cos(theta_2) - config.l4_x*sin(theta_2) + config.l4_y*sin(theta_3)*cos(theta_2) + config.l4_z*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_2) - config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*sin(theta_2) + config.m_ll*(-config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + (-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2)*cos(theta_1) - (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2)*cos(theta_1) - (config.Ixx_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*cos(theta_1)*cos(theta_2) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_1)*cos(theta_2) - (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*sin(theta_2) + (config.Ixx_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1))*thetadot4 + (config.m_foot*(-2*config.l3_x*sin(theta_2) + 2*config.l3_z*cos(theta_2) - 2*config.l4_x*sin(theta_2) + 2*config.l4_y*sin(theta_3)*cos(theta_2) + 2*config.l4_z*cos(theta_2)*cos(theta_3) - 2*config.l5_x*sin(theta_2) + 2*config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) - 2*config.l6_x_com*sin(theta_2) + 2*config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + 2*config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(2*config.l3_x*cos(theta_2) + 2*config.l3_z*sin(theta_2) + 2*config.l4_x*cos(theta_2) + 2*config.l4_y*sin(theta_2)*sin(theta_3) + 2*config.l4_z*sin(theta_2)*cos(theta_3) + 2*config.l5_x*cos(theta_2) + 2*config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + 2*config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + 2*config.l6_x_com*cos(theta_2) + 2*config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + 2*config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*pow(sin(theta_1), 2) + config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(2*config.l3_x*cos(theta_2) + 2*config.l3_z*sin(theta_2) + 2*config.l4_x*cos(theta_2) + 2*config.l4_y*sin(theta_2)*sin(theta_3) + 2*config.l4_z*sin(theta_2)*cos(theta_3) + 2*config.l5_x*cos(theta_2) + 2*config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + 2*config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + 2*config.l6_x_com*cos(theta_2) + 2*config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + 2*config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*pow(cos(theta_1), 2) + config.m_hip1*(-2*config.l3_x_com*sin(theta_2) + 2*config.l3_z_com*cos(theta_2))*(config.l3_x_com*cos(theta_2) + config.l3_z_com*sin(theta_2)) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(2*config.l3_x_com*cos(theta_2) + 2*config.l3_z_com*sin(theta_2))*pow(sin(theta_1), 2) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(2*config.l3_x_com*cos(theta_2) + 2*config.l3_z_com*sin(theta_2))*pow(cos(theta_1), 2) + config.m_ll*(-2*config.l3_x*sin(theta_2) + 2*config.l3_z*cos(theta_2) - 2*config.l4_x*sin(theta_2) + 2*config.l4_y*sin(theta_3)*cos(theta_2) + 2*config.l4_z*cos(theta_2)*cos(theta_3) - 2*config.l5_x*sin(theta_2) + 2*config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4)) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(2*config.l3_x*cos(theta_2) + 2*config.l3_z*sin(theta_2) + 2*config.l4_x*cos(theta_2) + 2*config.l4_y*sin(theta_2)*sin(theta_3) + 2*config.l4_z*sin(theta_2)*cos(theta_3) + 2*config.l5_x*cos(theta_2) + 2*config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + 2*config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*pow(sin(theta_1), 2) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(2*config.l3_x*cos(theta_2) + 2*config.l3_z*sin(theta_2) + 2*config.l4_x*cos(theta_2) + 2*config.l4_y*sin(theta_2)*sin(theta_3) + 2*config.l4_z*sin(theta_2)*cos(theta_3) + 2*config.l5_x*cos(theta_2) + 2*config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + 2*config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*pow(cos(theta_1), 2) + config.m_ul*(-2*config.l3_x*sin(theta_2) + 2*config.l3_z*cos(theta_2) - 2*config.l4_x_com*sin(theta_2) + 2*config.l4_y_com*sin(theta_3)*cos(theta_2) + 2*config.l4_z_com*cos(theta_2)*cos(theta_3))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3)) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(2*config.l3_x*cos(theta_2) + 2*config.l3_z*sin(theta_2) + 2*config.l4_x_com*cos(theta_2) + 2*config.l4_y_com*sin(theta_2)*sin(theta_3) + 2*config.l4_z_com*sin(theta_2)*cos(theta_3))*pow(sin(theta_1), 2) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(2*config.l3_x*cos(theta_2) + 2*config.l3_z*sin(theta_2) + 2*config.l4_x_com*cos(theta_2) + 2*config.l4_y_com*sin(theta_2)*sin(theta_3) + 2*config.l4_z_com*sin(theta_2)*cos(theta_3))*pow(cos(theta_1), 2) + (-config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1)*sin(theta_2) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1)*cos(theta_2))*cos(theta_1) + (config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_2)*cos(theta_1) - config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1)*cos(theta_2))*sin(theta_1) + (-(config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*sin(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*sin(theta_3)*cos(theta_2) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_2)*cos(theta_1) - (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_3)*cos(theta_1)*cos(theta_2) - (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1) + (-(config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*sin(theta_2) + (config.Iyx_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izx_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyy_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izy_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4)) + (config.Iyz_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izz_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*cos(theta_1) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_2)*cos(theta_1) - (config.Iyx_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izx_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*cos(theta_1)*cos(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3)) - (config.Iyy_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izy_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Iyz_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izz_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*sin(theta_1))*thetadot2 + (config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l3_x*sin(theta_1)*sin(theta_2) + config.l3_z*sin(theta_1)*cos(theta_2) - config.l4_x*sin(theta_1)*sin(theta_2) + config.l4_y*sin(theta_1)*sin(theta_3)*cos(theta_2) + config.l4_z*sin(theta_1)*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_1)*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) + config.l5_z*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x_com*sin(theta_1)*sin(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l3_x*sin(theta_2)*cos(theta_1) - config.l3_z*cos(theta_1)*cos(theta_2) + config.l4_x*sin(theta_2)*cos(theta_1) - config.l4_y*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l4_z*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2)*cos(theta_1) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l6_x_com*sin(theta_2)*cos(theta_1) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))*sin(theta_1) + config.m_foot*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*cos(theta_1) + config.m_foot*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*sin(theta_1) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(-config.l3_x_com*sin(theta_1)*sin(theta_2) + config.l3_z_com*sin(theta_1)*cos(theta_2))*cos(theta_1) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(config.l3_x_com*sin(theta_2)*cos(theta_1) - config.l3_z_com*cos(theta_1)*cos(theta_2))*sin(theta_1) + config.m_hip1*(config.l3_x_com*cos(theta_2) + config.l3_z_com*sin(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x_com*sin(theta_1)*cos(theta_2) + config.l3_y_com*cos(theta_1) + config.l3_z_com*sin(theta_1)*sin(theta_2))*cos(theta_1) + config.m_hip1*(config.l3_x_com*cos(theta_2) + config.l3_z_com*sin(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x_com*cos(theta_1)*cos(theta_2) + config.l3_y_com*sin(theta_1) - config.l3_z_com*sin(theta_2)*cos(theta_1))*sin(theta_1) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(-config.l3_x*sin(theta_1)*sin(theta_2) + config.l3_z*sin(theta_1)*cos(theta_2) - config.l4_x*sin(theta_1)*sin(theta_2) + config.l4_y*sin(theta_1)*sin(theta_3)*cos(theta_2) + config.l4_z*sin(theta_1)*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_1)*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) + config.l5_z*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(config.l3_x*sin(theta_2)*cos(theta_1) - config.l3_z*cos(theta_1)*cos(theta_2) + config.l4_x*sin(theta_2)*cos(theta_1) - config.l4_y*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l4_z*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2)*cos(theta_1) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))*sin(theta_1) + config.m_ll*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*cos(theta_1) + config.m_ll*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*sin(theta_1) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(-config.l3_x*sin(theta_1)*sin(theta_2) + config.l3_z*sin(theta_1)*cos(theta_2) - config.l4_x_com*sin(theta_1)*sin(theta_2) + config.l4_y_com*sin(theta_1)*sin(theta_3)*cos(theta_2) + config.l4_z_com*sin(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(config.l3_x*sin(theta_2)*cos(theta_1) - config.l3_z*cos(theta_1)*cos(theta_2) + config.l4_x_com*sin(theta_2)*cos(theta_1) - config.l4_y_com*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l4_z_com*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1) + config.m_ul*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*cos(theta_1) + config.m_ul*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*sin(theta_1) + (-(-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*sin(theta_1)*sin(theta_2) + (-config.Ixx_ul*cos(theta_2) - config.Izx_ul*sin(theta_2))*sin(theta_1)*cos(theta_2) + (-config.Ixy_ul*cos(theta_2) - config.Izy_ul*sin(theta_2))*cos(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_1)*cos(theta_2) + (-config.Ixz_ul*cos(theta_2) - config.Izz_ul*sin(theta_2))*sin(theta_1)*sin(theta_2))*cos(theta_1) + ((-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*sin(theta_2)*cos(theta_1) - (-config.Ixx_ul*cos(theta_2) - config.Izx_ul*sin(theta_2))*cos(theta_1)*cos(theta_2) - (config.Ixy_ul*cos(theta_2) + config.Izy_ul*sin(theta_2))*sin(theta_1) - (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*cos(theta_1)*cos(theta_2) - (-config.Ixz_ul*cos(theta_2) - config.Izz_ul*sin(theta_2))*sin(theta_2)*cos(theta_1))*sin(theta_1) + (-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*cos(theta_2) + config.Iyy_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izy_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*cos(theta_2) + config.Iyz_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izz_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) - (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2)*cos(theta_1) - (-config.Ixx_foot*cos(theta_2) + config.Iyx_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izx_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*sin(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*cos(theta_2) + config.Iyz_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izz_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*cos(theta_2) + config.Iyy_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izy_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*sin(theta_2) + (-config.Ixx_foot*cos(theta_2) + config.Iyx_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izx_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*cos(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(-config.Ixz_ll*cos(theta_2) - config.Iyz_ll*sin(theta_2)*sin(theta_3) - config.Izz_ll*sin(theta_2)*cos(theta_3)) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(-config.Ixy_ll*cos(theta_2) - config.Iyy_ll*sin(theta_2)*sin(theta_3) - config.Izy_ll*sin(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (-config.Ixx_ll*cos(theta_2) - config.Iyx_ll*sin(theta_2)*sin(theta_3) - config.Izx_ll*sin(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2) - (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*sin(theta_3)*cos(theta_1)*cos(theta_2) - (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*cos(theta_2) - config.Iyy_ll*sin(theta_2)*sin(theta_3) - config.Izy_ll*sin(theta_2)*cos(theta_3)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*cos(theta_2) - config.Iyz_ll*sin(theta_2)*sin(theta_3) - config.Izz_ll*sin(theta_2)*cos(theta_3)) - (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*sin(theta_2) + (-config.Ixx_ll*cos(theta_2) - config.Iyx_ll*sin(theta_2)*sin(theta_3) - config.Izx_ll*sin(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*sin(theta_3)*cos(theta_2) + (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1))*thetadot1 + (config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_1) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*sin(theta_1) + config.m_foot*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l3_x*sin(theta_2) + config.l3_z*cos(theta_2) - config.l4_x*sin(theta_2) + config.l4_y*sin(theta_3)*cos(theta_2) + config.l4_z*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x_com*sin(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_2) - config.m_foot*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*sin(theta_2) + config.m_foot*(-config.l4_y*sin(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_1)*sin(theta_3)*cos(theta_2) - config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) - config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l4_y*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_1) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*sin(theta_1) + config.m_ll*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(-config.l3_x*sin(theta_2) + config.l3_z*cos(theta_2) - config.l4_x*sin(theta_2) + config.l4_y*sin(theta_3)*cos(theta_2) + config.l4_z*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_2) - config.m_ll*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*sin(theta_2) + config.m_ll*(-config.l4_y*sin(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_1)*sin(theta_3)*cos(theta_2) - config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l4_y*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3))*cos(theta_1) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3))*sin(theta_1) + config.m_ul*(-config.l4_y_com*cos(theta_3) + config.l4_z_com*sin(theta_3))*(-config.l3_x*sin(theta_2) + config.l3_z*cos(theta_2) - config.l4_x_com*sin(theta_2) + config.l4_y_com*sin(theta_3)*cos(theta_2) + config.l4_z_com*cos(theta_2)*cos(theta_3))*cos(theta_2) - config.m_ul*(-config.l4_y_com*cos(theta_3) + config.l4_z_com*sin(theta_3))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3))*sin(theta_2) + config.m_ul*(-config.l4_y_com*sin(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z_com*sin(theta_1)*sin(theta_3)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*sin(theta_1) + config.m_ul*(-config.l4_y_com*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z_com*sin(theta_3)*cos(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-(config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*sin(theta_2) + (config.Ixx_ul*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Izx_ul*(-pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - pow(cos(theta_2), 2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ul*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Izy_ul*(-pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - pow(cos(theta_2), 2)))*cos(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixz_ul*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Izz_ul*(-pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - pow(cos(theta_2), 2)))*sin(theta_1)*sin(theta_2))*cos(theta_1) + ((config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_2)*cos(theta_1) - (config.Ixx_ul*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Izx_ul*(-pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - pow(cos(theta_2), 2)))*cos(theta_1)*cos(theta_2) - (-config.Ixy_ul*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Izy_ul*(-pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - pow(cos(theta_2), 2)))*sin(theta_1) - (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Ixz_ul*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Izz_ul*(-pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - pow(cos(theta_2), 2)))*sin(theta_2)*cos(theta_1))*sin(theta_1) + (-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2)*cos(theta_1) - (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2))*cos(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2)*cos(theta_1) - (config.Ixx_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*cos(theta_1)*cos(theta_2) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_1)*cos(theta_2) - (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*sin(theta_2) + (config.Ixx_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1))*thetadot3)*thetadot2 - 0.5L*((config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-2*config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + 2*config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-2*config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + 2*config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)) - 2*config.m_foot*pow(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5), 2)*sin(theta_2)*cos(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_2)*cos(theta_1) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*cos(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2)*cos(theta_1) + (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + (-(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_2) - (-config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2))*sin(theta_2))*thetadot5 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l3_x*sin(theta_1)*sin(theta_2) + config.l3_z*sin(theta_1)*cos(theta_2) - config.l4_x*sin(theta_1)*sin(theta_2) + config.l4_y*sin(theta_1)*sin(theta_3)*cos(theta_2) + config.l4_z*sin(theta_1)*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_1)*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) + config.l5_z*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x_com*sin(theta_1)*sin(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2)*cos(theta_1) - config.l3_z*cos(theta_1)*cos(theta_2) + config.l4_x*sin(theta_2)*cos(theta_1) - config.l4_y*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l4_z*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2)*cos(theta_1) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l6_x_com*sin(theta_2)*cos(theta_1) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.m_foot*(-config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*sin(theta_2)*cos(theta_1) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2))*cos(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*cos(theta_2) + config.Iyy_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izy_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*cos(theta_2) + config.Iyz_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izz_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2)*cos(theta_1) + (-config.Ixx_foot*cos(theta_2) + config.Iyx_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izx_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*cos(theta_2) + config.Iyz_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izz_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*cos(theta_2) + config.Iyy_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izy_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*sin(theta_2) + (-config.Ixx_foot*cos(theta_2) + config.Iyx_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izx_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + (-(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*cos(theta_2) + config.Iyz_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izz_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*cos(theta_2) + config.Iyy_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izy_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) - (config.Ixx_foot*sin(theta_2) - config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_2) - (config.Ixx_foot*cos(theta_2) - config.Iyx_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) - config.Izx_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2))*sin(theta_2))*thetadot1 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) - config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)) - 2*config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*sin(theta_2)*cos(theta_2) + config.m_foot*(-config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_2)*cos(theta_1) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*cos(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2)*cos(theta_1) + (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + (-(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_2) - (-config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2))*sin(theta_2))*thetadot4 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*sin(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_1)*sin(theta_3)*cos(theta_2) - config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) - config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)) - 2*config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*sin(theta_2)*cos(theta_2) + config.m_foot*(-config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_2)*cos(theta_1) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*cos(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2)*cos(theta_1) + (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + (-(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_2) - (-config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2))*sin(theta_2))*thetadot3 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*sin(theta_1) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l3_x*sin(theta_2) + config.l3_z*cos(theta_2) - config.l4_x*sin(theta_2) + config.l4_y*sin(theta_3)*cos(theta_2) + config.l4_z*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x_com*sin(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_2) - config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*sin(theta_2) + config.m_foot*(-config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_2) - ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1)*sin(theta_2) - ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*sin(theta_2)*cos(theta_1) + (-(-config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_2) - (-config.Iyx_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) - config.Izx_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*sin(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) - (config.Iyy_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izy_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Iyz_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izz_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) + (-(config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*sin(theta_2) + (config.Iyx_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izx_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyy_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izy_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4)) + (config.Iyz_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izz_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1)*cos(theta_2) + (-(config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_2)*cos(theta_1) + (config.Iyx_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izx_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3)) + (config.Iyy_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izy_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4)) + (config.Iyz_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izz_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1)*cos(theta_2))*thetadot2)*thetadot5 - 0.5L*((config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l3_x*sin(theta_1)*sin(theta_2) + config.l3_z*sin(theta_1)*cos(theta_2) - config.l4_x*sin(theta_1)*sin(theta_2) + config.l4_y*sin(theta_1)*sin(theta_3)*cos(theta_2) + config.l4_z*sin(theta_1)*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_1)*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) + config.l5_z*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x_com*sin(theta_1)*sin(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2)*cos(theta_1) - config.l3_z*cos(theta_1)*cos(theta_2) + config.l4_x*sin(theta_2)*cos(theta_1) - config.l4_y*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l4_z*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2)*cos(theta_1) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l6_x_com*sin(theta_2)*cos(theta_1) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.m_foot*(-config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + (sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_2) + (-config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2))*thetadot5 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l3_x*sin(theta_1)*sin(theta_2) + config.l3_z*sin(theta_1)*cos(theta_2) - config.l4_x*sin(theta_1)*sin(theta_2) + config.l4_y*sin(theta_1)*sin(theta_3)*cos(theta_2) + config.l4_z*sin(theta_1)*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_1)*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) + config.l5_z*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x_com*sin(theta_1)*sin(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2)*cos(theta_1) - config.l3_z*cos(theta_1)*cos(theta_2) + config.l4_x*sin(theta_2)*cos(theta_1) - config.l4_y*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l4_z*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2)*cos(theta_1) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l6_x_com*sin(theta_2)*cos(theta_1) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.m_foot*(-config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) - config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l3_x*sin(theta_1)*sin(theta_2) + config.l3_z*sin(theta_1)*cos(theta_2) - config.l4_x*sin(theta_1)*sin(theta_2) + config.l4_y*sin(theta_1)*sin(theta_3)*cos(theta_2) + config.l4_z*sin(theta_1)*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_1)*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) + config.l5_z*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2)*cos(theta_1) - config.l3_z*cos(theta_1)*cos(theta_2) + config.l4_x*sin(theta_2)*cos(theta_1) - config.l4_y*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l4_z*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2)*cos(theta_1) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.m_ll*(-config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + (sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_2) + (-config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2) + (-config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) - config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2) + (-config.Ixx_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) - config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*sin(theta_2) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2)*sin(theta_3) + (config.Ixy_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*sin(theta_3)*cos(theta_2) - (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2)*cos(theta_3) + (config.Ixz_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*cos(theta_2)*cos(theta_3))*thetadot4 + (config.m_foot*(-2*config.l3_x*sin(theta_1)*sin(theta_2) + 2*config.l3_z*sin(theta_1)*cos(theta_2) - 2*config.l4_x*sin(theta_1)*sin(theta_2) + 2*config.l4_y*sin(theta_1)*sin(theta_3)*cos(theta_2) + 2*config.l4_z*sin(theta_1)*cos(theta_2)*cos(theta_3) - 2*config.l5_x*sin(theta_1)*sin(theta_2) + 2*config.l5_y*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) + 2*config.l5_z*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) - 2*config.l6_x_com*sin(theta_1)*sin(theta_2) + 2*config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2) + 2*config.l6_z_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(2*config.l3_x*sin(theta_2)*cos(theta_1) - 2*config.l3_z*cos(theta_1)*cos(theta_2) + 2*config.l4_x*sin(theta_2)*cos(theta_1) - 2*config.l4_y*sin(theta_3)*cos(theta_1)*cos(theta_2) - 2*config.l4_z*cos(theta_1)*cos(theta_2)*cos(theta_3) + 2*config.l5_x*sin(theta_2)*cos(theta_1) - 2*config.l5_y*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - 2*config.l5_z*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + 2*config.l6_x_com*sin(theta_2)*cos(theta_1) - 2*config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) - 2*config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_hip1*(-2*config.l3_x_com*sin(theta_1)*sin(theta_2) + 2*config.l3_z_com*sin(theta_1)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x_com*sin(theta_1)*cos(theta_2) + config.l3_y_com*cos(theta_1) + config.l3_z_com*sin(theta_1)*sin(theta_2)) + config.m_hip1*(2*config.l3_x_com*sin(theta_2)*cos(theta_1) - 2*config.l3_z_com*cos(theta_1)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x_com*cos(theta_1)*cos(theta_2) + config.l3_y_com*sin(theta_1) - config.l3_z_com*sin(theta_2)*cos(theta_1)) + config.m_ll*(-2*config.l3_x*sin(theta_1)*sin(theta_2) + 2*config.l3_z*sin(theta_1)*cos(theta_2) - 2*config.l4_x*sin(theta_1)*sin(theta_2) + 2*config.l4_y*sin(theta_1)*sin(theta_3)*cos(theta_2) + 2*config.l4_z*sin(theta_1)*cos(theta_2)*cos(theta_3) - 2*config.l5_x*sin(theta_1)*sin(theta_2) + 2*config.l5_y*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) + 2*config.l5_z*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ll*(2*config.l3_x*sin(theta_2)*cos(theta_1) - 2*config.l3_z*cos(theta_1)*cos(theta_2) + 2*config.l4_x*sin(theta_2)*cos(theta_1) - 2*config.l4_y*sin(theta_3)*cos(theta_1)*cos(theta_2) - 2*config.l4_z*cos(theta_1)*cos(theta_2)*cos(theta_3) + 2*config.l5_x*sin(theta_2)*cos(theta_1) - 2*config.l5_y*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - 2*config.l5_z*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ul*(-2*config.l3_x*sin(theta_1)*sin(theta_2) + 2*config.l3_z*sin(theta_1)*cos(theta_2) - 2*config.l4_x_com*sin(theta_1)*sin(theta_2) + 2*config.l4_y_com*sin(theta_1)*sin(theta_3)*cos(theta_2) + 2*config.l4_z_com*sin(theta_1)*cos(theta_2)*cos(theta_3))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))) + config.m_ul*(2*config.l3_x*sin(theta_2)*cos(theta_1) - 2*config.l3_z*cos(theta_1)*cos(theta_2) + 2*config.l4_x_com*sin(theta_2)*cos(theta_1) - 2*config.l4_y_com*sin(theta_3)*cos(theta_1)*cos(theta_2) - 2*config.l4_z_com*cos(theta_1)*cos(theta_2)*cos(theta_3))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))) + (config.Ixx_ul*sin(theta_2) - config.Izx_ul*cos(theta_2))*cos(theta_2) + (config.Ixx_ul*cos(theta_2) + config.Izx_ul*sin(theta_2))*sin(theta_2) - (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_2) + (-config.Ixz_ul*cos(theta_2) - config.Izz_ul*sin(theta_2))*cos(theta_2) + (sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*cos(theta_2) + config.Iyz_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izz_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*cos(theta_2) + config.Iyy_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izy_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) + (config.Ixx_foot*sin(theta_2) - config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_2) + (config.Ixx_foot*cos(theta_2) - config.Iyx_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) - config.Izx_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) + (config.Ixx_ll*sin(theta_2) - config.Iyx_ll*sin(theta_3)*cos(theta_2) - config.Izx_ll*cos(theta_2)*cos(theta_3))*cos(theta_2) + (config.Ixx_ll*cos(theta_2) + config.Iyx_ll*sin(theta_2)*sin(theta_3) + config.Izx_ll*sin(theta_2)*cos(theta_3))*sin(theta_2) - (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*sin(theta_2)*sin(theta_3) + (-config.Ixy_ll*cos(theta_2) - config.Iyy_ll*sin(theta_2)*sin(theta_3) - config.Izy_ll*sin(theta_2)*cos(theta_3))*sin(theta_3)*cos(theta_2) - (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*sin(theta_2)*cos(theta_3) + (-config.Ixz_ll*cos(theta_2) - config.Iyz_ll*sin(theta_2)*sin(theta_3) - config.Izz_ll*sin(theta_2)*cos(theta_3))*cos(theta_2)*cos(theta_3))*thetadot1 + (-config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_2) - config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_2) + config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l3_x*sin(theta_1)*sin(theta_2) + config.l3_z*sin(theta_1)*cos(theta_2) - config.l4_x*sin(theta_1)*sin(theta_2) + config.l4_y*sin(theta_1)*sin(theta_3)*cos(theta_2) + config.l4_z*sin(theta_1)*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_1)*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) + config.l5_z*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x_com*sin(theta_1)*sin(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l3_x*sin(theta_2)*cos(theta_1) - config.l3_z*cos(theta_1)*cos(theta_2) + config.l4_x*sin(theta_2)*cos(theta_1) - config.l4_y*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l4_z*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2)*cos(theta_1) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l6_x_com*sin(theta_2)*cos(theta_1) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))*sin(theta_1) + config.m_foot*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*cos(theta_1) + config.m_foot*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*sin(theta_1) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(-config.l3_x_com*sin(theta_1)*sin(theta_2) + config.l3_z_com*sin(theta_1)*cos(theta_2))*cos(theta_1) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(config.l3_x_com*sin(theta_2)*cos(theta_1) - config.l3_z_com*cos(theta_1)*cos(theta_2))*sin(theta_1) + config.m_hip1*(config.l3_x_com*cos(theta_2) + config.l3_z_com*sin(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x_com*sin(theta_1)*cos(theta_2) + config.l3_y_com*cos(theta_1) + config.l3_z_com*sin(theta_1)*sin(theta_2))*cos(theta_1) + config.m_hip1*(config.l3_x_com*cos(theta_2) + config.l3_z_com*sin(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x_com*cos(theta_1)*cos(theta_2) + config.l3_y_com*sin(theta_1) - config.l3_z_com*sin(theta_2)*cos(theta_1))*sin(theta_1) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(-config.l3_x*sin(theta_1)*sin(theta_2) + config.l3_z*sin(theta_1)*cos(theta_2) - config.l4_x*sin(theta_1)*sin(theta_2) + config.l4_y*sin(theta_1)*sin(theta_3)*cos(theta_2) + config.l4_z*sin(theta_1)*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_1)*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) + config.l5_z*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(config.l3_x*sin(theta_2)*cos(theta_1) - config.l3_z*cos(theta_1)*cos(theta_2) + config.l4_x*sin(theta_2)*cos(theta_1) - config.l4_y*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l4_z*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2)*cos(theta_1) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))*sin(theta_1) + config.m_ll*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*cos(theta_1) + config.m_ll*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*sin(theta_1) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(-config.l3_x*sin(theta_1)*sin(theta_2) + config.l3_z*sin(theta_1)*cos(theta_2) - config.l4_x_com*sin(theta_1)*sin(theta_2) + config.l4_y_com*sin(theta_1)*sin(theta_3)*cos(theta_2) + config.l4_z_com*sin(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(config.l3_x*sin(theta_2)*cos(theta_1) - config.l3_z*cos(theta_1)*cos(theta_2) + config.l4_x_com*sin(theta_2)*cos(theta_1) - config.l4_y_com*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l4_z_com*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1) + config.m_ul*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*cos(theta_1) + config.m_ul*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*sin(theta_1) + (-config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_2) + (-config.Iyx_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) - config.Izx_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*sin(theta_2) + (-config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) - config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + (config.Iyy_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izy_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_2)*sin(theta_3) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4)) + (config.Iyz_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izz_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_2)*cos(theta_3))*thetadot2 + (config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l3_x*sin(theta_1)*sin(theta_2) + config.l3_z*sin(theta_1)*cos(theta_2) - config.l4_x*sin(theta_1)*sin(theta_2) + config.l4_y*sin(theta_1)*sin(theta_3)*cos(theta_2) + config.l4_z*sin(theta_1)*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_1)*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) + config.l5_z*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x_com*sin(theta_1)*sin(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2)*cos(theta_1) - config.l3_z*cos(theta_1)*cos(theta_2) + config.l4_x*sin(theta_2)*cos(theta_1) - config.l4_y*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l4_z*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2)*cos(theta_1) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l6_x_com*sin(theta_2)*cos(theta_1) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.m_foot*(-config.l4_y*sin(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_1)*sin(theta_3)*cos(theta_2) - config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) - config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l4_y*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l3_x*sin(theta_1)*sin(theta_2) + config.l3_z*sin(theta_1)*cos(theta_2) - config.l4_x*sin(theta_1)*sin(theta_2) + config.l4_y*sin(theta_1)*sin(theta_3)*cos(theta_2) + config.l4_z*sin(theta_1)*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_1)*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) + config.l5_z*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2)*cos(theta_1) - config.l3_z*cos(theta_1)*cos(theta_2) + config.l4_x*sin(theta_2)*cos(theta_1) - config.l4_y*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l4_z*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2)*cos(theta_1) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.m_ll*(-config.l4_y*sin(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_1)*sin(theta_3)*cos(theta_2) - config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l4_y*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(-config.l3_x*sin(theta_1)*sin(theta_2) + config.l3_z*sin(theta_1)*cos(theta_2) - config.l4_x_com*sin(theta_1)*sin(theta_2) + config.l4_y_com*sin(theta_1)*sin(theta_3)*cos(theta_2) + config.l4_z_com*sin(theta_1)*cos(theta_2)*cos(theta_3)) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(config.l3_x*sin(theta_2)*cos(theta_1) - config.l3_z*cos(theta_1)*cos(theta_2) + config.l4_x_com*sin(theta_2)*cos(theta_1) - config.l4_y_com*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l4_z_com*cos(theta_1)*cos(theta_2)*cos(theta_3)) + config.m_ul*(-config.l4_y_com*sin(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z_com*sin(theta_1)*sin(theta_3)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))) + config.m_ul*(-config.l4_y_com*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z_com*sin(theta_3)*cos(theta_1)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))) + (-config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_2) + (-config.Ixx_ul*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Izx_ul*(-pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - pow(cos(theta_2), 2)))*sin(theta_2) - (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_2) + (config.Ixz_ul*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Izz_ul*(-pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - pow(cos(theta_2), 2)))*cos(theta_2) + (sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_2) + (-config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2) + (-config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) - config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2) + (-config.Ixx_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) - config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*sin(theta_2) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2)*sin(theta_3) + (config.Ixy_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*sin(theta_3)*cos(theta_2) - (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2)*cos(theta_3) + (config.Ixz_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*cos(theta_2)*cos(theta_3))*thetadot3)*thetadot1 - 0.5L*((config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) - config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)) - 2*config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*sin(theta_2)*cos(theta_2) + config.m_foot*(-config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_2)*cos(theta_1) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*cos(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2)*cos(theta_1) + (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + (-(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_2) - (-config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2))*sin(theta_2))*thetadot5 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-2*config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + 2*config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - 2*config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + 2*config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-2*config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) - 2*config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + 2*config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)) - 2*config.m_foot*pow(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5), 2)*sin(theta_2)*cos(theta_2) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-2*config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + 2*config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-2*config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - 2*config.m_ll*pow(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4), 2)*sin(theta_2)*cos(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_2)*cos(theta_1) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*sin(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*cos(theta_2) + ((config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_2) - (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2)*cos(theta_3))*cos(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2)*cos(theta_1) + (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2)*cos(theta_1) + (config.Ixx_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*cos(theta_1)*cos(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_1)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*sin(theta_2) + (config.Ixx_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_2) - (-config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2))*sin(theta_2) + (-(-config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) - config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2) - (-config.Ixx_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) - config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*sin(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2)*sin(theta_3) - (config.Ixy_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2)*cos(theta_3) - (config.Ixz_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*cos(theta_2)*cos(theta_3))*sin(theta_2))*thetadot4 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l3_x*sin(theta_1)*sin(theta_2) + config.l3_z*sin(theta_1)*cos(theta_2) - config.l4_x*sin(theta_1)*sin(theta_2) + config.l4_y*sin(theta_1)*sin(theta_3)*cos(theta_2) + config.l4_z*sin(theta_1)*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_1)*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) + config.l5_z*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x_com*sin(theta_1)*sin(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2)*cos(theta_1) - config.l3_z*cos(theta_1)*cos(theta_2) + config.l4_x*sin(theta_2)*cos(theta_1) - config.l4_y*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l4_z*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2)*cos(theta_1) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l6_x_com*sin(theta_2)*cos(theta_1) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.m_foot*(-config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) - config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l3_x*sin(theta_1)*sin(theta_2) + config.l3_z*sin(theta_1)*cos(theta_2) - config.l4_x*sin(theta_1)*sin(theta_2) + config.l4_y*sin(theta_1)*sin(theta_3)*cos(theta_2) + config.l4_z*sin(theta_1)*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_1)*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) + config.l5_z*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2)*cos(theta_1) - config.l3_z*cos(theta_1)*cos(theta_2) + config.l4_x*sin(theta_2)*cos(theta_1) - config.l4_y*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l4_z*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2)*cos(theta_1) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.m_ll*(-config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*sin(theta_2)*cos(theta_1) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*sin(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2))*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2))*cos(theta_2) + ((-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_2) - (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*sin(theta_3)*cos(theta_2) - (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*cos(theta_2)*cos(theta_3))*cos(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*cos(theta_2) + config.Iyy_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izy_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*cos(theta_2) + config.Iyz_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izz_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2)*cos(theta_1) + (-config.Ixx_foot*cos(theta_2) + config.Iyx_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izx_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*cos(theta_2) + config.Iyz_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izz_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*cos(theta_2) + config.Iyy_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izy_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*sin(theta_2) + (-config.Ixx_foot*cos(theta_2) + config.Iyx_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izx_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(-config.Ixz_ll*cos(theta_2) - config.Iyz_ll*sin(theta_2)*sin(theta_3) - config.Izz_ll*sin(theta_2)*cos(theta_3)) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(-config.Ixy_ll*cos(theta_2) - config.Iyy_ll*sin(theta_2)*sin(theta_3) - config.Izy_ll*sin(theta_2)*cos(theta_3)) - (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_2)*cos(theta_1) + (-config.Ixx_ll*cos(theta_2) - config.Iyx_ll*sin(theta_2)*sin(theta_3) - config.Izx_ll*sin(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*sin(theta_3)*cos(theta_1)*cos(theta_2) + (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*cos(theta_2) - config.Iyy_ll*sin(theta_2)*sin(theta_3) - config.Izy_ll*sin(theta_2)*cos(theta_3)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*cos(theta_2) - config.Iyz_ll*sin(theta_2)*sin(theta_3) - config.Izz_ll*sin(theta_2)*cos(theta_3)) - (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*sin(theta_2) + (-config.Ixx_ll*cos(theta_2) - config.Iyx_ll*sin(theta_2)*sin(theta_3) - config.Izx_ll*sin(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*sin(theta_3)*cos(theta_2) + (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*cos(theta_2) + config.Iyz_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izz_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*cos(theta_2) + config.Iyy_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izy_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) - (config.Ixx_foot*sin(theta_2) - config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_2) - (config.Ixx_foot*cos(theta_2) - config.Iyx_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) - config.Izx_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2))*sin(theta_2) + (-(config.Ixx_ll*sin(theta_2) - config.Iyx_ll*sin(theta_3)*cos(theta_2) - config.Izx_ll*cos(theta_2)*cos(theta_3))*cos(theta_2) - (config.Ixx_ll*cos(theta_2) + config.Iyx_ll*sin(theta_2)*sin(theta_3) + config.Izx_ll*sin(theta_2)*cos(theta_3))*sin(theta_2) + (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*sin(theta_2)*sin(theta_3) - (-config.Ixy_ll*cos(theta_2) - config.Iyy_ll*sin(theta_2)*sin(theta_3) - config.Izy_ll*sin(theta_2)*cos(theta_3))*sin(theta_3)*cos(theta_2) + (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*sin(theta_2)*cos(theta_3) - (-config.Ixz_ll*cos(theta_2) - config.Iyz_ll*sin(theta_2)*sin(theta_3) - config.Izz_ll*sin(theta_2)*cos(theta_3))*cos(theta_2)*cos(theta_3))*sin(theta_2))*thetadot1 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*sin(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_1)*sin(theta_3)*cos(theta_2) - config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) - config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)) - 2*config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*sin(theta_2)*cos(theta_2) + config.m_foot*(-config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) - config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l4_y*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*sin(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_1)*sin(theta_3)*cos(theta_2) - config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - 2*config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*sin(theta_2)*cos(theta_2) + config.m_ll*(-config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4))) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_2)*cos(theta_1) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*sin(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*cos(theta_2) + ((config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_2) - (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2)*cos(theta_3))*cos(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2)*cos(theta_1) + (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2)*cos(theta_1) + (config.Ixx_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*cos(theta_1)*cos(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_1)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*sin(theta_2) + (config.Ixx_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_2) - (-config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2))*sin(theta_2) + (-(-config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) - config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2) - (-config.Ixx_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) - config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*sin(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2)*sin(theta_3) - (config.Ixy_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2)*cos(theta_3) - (config.Ixz_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*cos(theta_2)*cos(theta_3))*sin(theta_2))*thetadot3 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_1) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*sin(theta_1) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l3_x*sin(theta_2) + config.l3_z*cos(theta_2) - config.l4_x*sin(theta_2) + config.l4_y*sin(theta_3)*cos(theta_2) + config.l4_z*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x_com*sin(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_2) - config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*sin(theta_2) + config.m_foot*(-config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) - config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_1) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*sin(theta_1) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(-config.l3_x*sin(theta_2) + config.l3_z*cos(theta_2) - config.l4_x*sin(theta_2) + config.l4_y*sin(theta_3)*cos(theta_2) + config.l4_z*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_2) - config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*sin(theta_2) + config.m_ll*(-config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_2) + (-(-config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) - config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_2)*sin(theta_3) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_2)*cos(theta_3))*sin(theta_2) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_2) - (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_3)*cos(theta_2) - (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_2)*cos(theta_3))*cos(theta_2) - ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1)*sin(theta_2) - ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*sin(theta_2)*cos(theta_1) + (-(config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*sin(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*sin(theta_3)*cos(theta_2) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) - ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*sin(theta_1)*sin(theta_2) + (-(config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_2)*cos(theta_1) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_3)*cos(theta_1)*cos(theta_2) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2) - ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*sin(theta_2)*cos(theta_1) + (-(-config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_2) - (-config.Iyx_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) - config.Izx_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*sin(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) - (config.Iyy_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izy_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Iyz_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izz_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) + (-(config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*sin(theta_2) + (config.Iyx_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izx_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyy_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izy_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4)) + (config.Iyz_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izz_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1)*cos(theta_2) + (-(config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_2)*cos(theta_1) + (config.Iyx_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izx_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3)) + (config.Iyy_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izy_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4)) + (config.Iyz_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izz_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1)*cos(theta_2))*thetadot2)*thetadot4 - 0.5L*((config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*sin(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_1)*sin(theta_3)*cos(theta_2) - config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) - config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)) - 2*config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*sin(theta_2)*cos(theta_2) + config.m_foot*(-config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_2)*cos(theta_1) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*cos(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2)*cos(theta_1) + (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + (-(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_2) - (-config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2))*sin(theta_2))*thetadot5 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*sin(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_1)*sin(theta_3)*cos(theta_2) - config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) - config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)) - 2*config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*sin(theta_2)*cos(theta_2) + config.m_foot*(-config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) - config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l4_y*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*sin(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_1)*sin(theta_3)*cos(theta_2) - config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - 2*config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*sin(theta_2)*cos(theta_2) + config.m_ll*(-config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4))) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_2)*cos(theta_1) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*sin(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*cos(theta_2) + ((config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_2) - (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2)*cos(theta_3))*cos(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2)*cos(theta_1) + (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2)*cos(theta_1) + (config.Ixx_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*cos(theta_1)*cos(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_1)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*sin(theta_2) + (config.Ixx_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_2) - (-config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2))*sin(theta_2) + (-(-config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) - config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2) - (-config.Ixx_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) - config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*sin(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2)*sin(theta_3) - (config.Ixy_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2)*cos(theta_3) - (config.Ixz_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*cos(theta_2)*cos(theta_3))*sin(theta_2))*thetadot4 + (config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-2*config.l4_y*cos(theta_1)*cos(theta_2)*cos(theta_3) + 2*config.l4_z*sin(theta_3)*cos(theta_1)*cos(theta_2) - 2*config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + 2*config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - 2*config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + 2*config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-2*config.l4_y*sin(theta_1)*cos(theta_2)*cos(theta_3) + 2*config.l4_z*sin(theta_1)*sin(theta_3)*cos(theta_2) - 2*config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) - 2*config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + 2*config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)) - 2*config.m_foot*pow(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5), 2)*sin(theta_2)*cos(theta_2) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-2*config.l4_y*cos(theta_1)*cos(theta_2)*cos(theta_3) + 2*config.l4_z*sin(theta_3)*cos(theta_1)*cos(theta_2) - 2*config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + 2*config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-2*config.l4_y*sin(theta_1)*cos(theta_2)*cos(theta_3) + 2*config.l4_z*sin(theta_1)*sin(theta_3)*cos(theta_2) - 2*config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - 2*config.m_ll*pow(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4), 2)*sin(theta_2)*cos(theta_2) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(-2*config.l4_y_com*cos(theta_1)*cos(theta_2)*cos(theta_3) + 2*config.l4_z_com*sin(theta_3)*cos(theta_1)*cos(theta_2)) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(-2*config.l4_y_com*sin(theta_1)*cos(theta_2)*cos(theta_3) + 2*config.l4_z_com*sin(theta_1)*sin(theta_3)*cos(theta_2)) - 2*config.m_ul*pow(-config.l4_y_com*cos(theta_3) + config.l4_z_com*sin(theta_3), 2)*sin(theta_2)*cos(theta_2) + ((config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_2) - (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_2))*cos(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*sin(theta_2)*cos(theta_1) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*sin(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*cos(theta_2) + ((config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_2) - (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2)*cos(theta_3))*cos(theta_2) - ((config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*sin(theta_2))*sin(theta_1)*sin(theta_2) - ((config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_2)*cos(theta_1))*sin(theta_2)*cos(theta_1) + (-(-config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_2) - (-config.Ixx_ul*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Izx_ul*(-pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - pow(cos(theta_2), 2)))*sin(theta_2) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_2) - (config.Ixz_ul*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Izz_ul*(-pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - pow(cos(theta_2), 2)))*cos(theta_2))*sin(theta_2) + (-(config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*sin(theta_2) + (config.Ixx_ul*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Izx_ul*(-pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - pow(cos(theta_2), 2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ul*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Izy_ul*(-pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - pow(cos(theta_2), 2)))*cos(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixz_ul*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Izz_ul*(-pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - pow(cos(theta_2), 2)))*sin(theta_1)*sin(theta_2))*sin(theta_1)*cos(theta_2) + (-(config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_2)*cos(theta_1) + (config.Ixx_ul*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Izx_ul*(-pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - pow(cos(theta_2), 2)))*cos(theta_1)*cos(theta_2) + (-config.Ixy_ul*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Izy_ul*(-pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - pow(cos(theta_2), 2)))*sin(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Ixz_ul*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Izz_ul*(-pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - pow(cos(theta_2), 2)))*sin(theta_2)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2)*cos(theta_1) + (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2)*cos(theta_1) + (config.Ixx_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*cos(theta_1)*cos(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_1)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*sin(theta_2) + (config.Ixx_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2))) - (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_2) - (-config.Ixx_foot*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*sin(theta_2) + (sin(theta_2)*sin(theta_3)*cos(theta_4) + sin(theta_2)*sin(theta_4)*cos(theta_3))*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_2) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_2)*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*sin(theta_2) + (-sin(theta_2)*sin(theta_3)*sin(theta_4) + sin(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_2) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1)*cos(theta_2)))*sin(theta_2))*sin(theta_2) + (-(-config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) - config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2) - (-config.Ixx_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) - config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) - config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*sin(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2)*sin(theta_3) - (config.Ixy_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2)*cos(theta_3) - (config.Ixz_ll*(-2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*sin(theta_3)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2)*sin(theta_3) + sin(theta_3)*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - sin(theta_3)*pow(cos(theta_2), 2)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_2)*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*sin(theta_2) + pow(sin(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) + pow(sin(theta_2), 2)*cos(theta_3) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)*cos(theta_3) - pow(cos(theta_2), 2)*cos(theta_3)))*cos(theta_2)*cos(theta_3))*sin(theta_2))*thetadot3 + (config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l3_x*sin(theta_1)*sin(theta_2) + config.l3_z*sin(theta_1)*cos(theta_2) - config.l4_x*sin(theta_1)*sin(theta_2) + config.l4_y*sin(theta_1)*sin(theta_3)*cos(theta_2) + config.l4_z*sin(theta_1)*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_1)*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) + config.l5_z*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x_com*sin(theta_1)*sin(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2)*cos(theta_1) - config.l3_z*cos(theta_1)*cos(theta_2) + config.l4_x*sin(theta_2)*cos(theta_1) - config.l4_y*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l4_z*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2)*cos(theta_1) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l6_x_com*sin(theta_2)*cos(theta_1) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.m_foot*(-config.l4_y*sin(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_1)*sin(theta_3)*cos(theta_2) - config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) - config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l4_y*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l3_x*sin(theta_1)*sin(theta_2) + config.l3_z*sin(theta_1)*cos(theta_2) - config.l4_x*sin(theta_1)*sin(theta_2) + config.l4_y*sin(theta_1)*sin(theta_3)*cos(theta_2) + config.l4_z*sin(theta_1)*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_1)*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) + config.l5_z*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2)*cos(theta_1) - config.l3_z*cos(theta_1)*cos(theta_2) + config.l4_x*sin(theta_2)*cos(theta_1) - config.l4_y*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l4_z*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2)*cos(theta_1) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.m_ll*(-config.l4_y*sin(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_1)*sin(theta_3)*cos(theta_2) - config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l4_y*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(-config.l3_x*sin(theta_1)*sin(theta_2) + config.l3_z*sin(theta_1)*cos(theta_2) - config.l4_x_com*sin(theta_1)*sin(theta_2) + config.l4_y_com*sin(theta_1)*sin(theta_3)*cos(theta_2) + config.l4_z_com*sin(theta_1)*cos(theta_2)*cos(theta_3)) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(config.l3_x*sin(theta_2)*cos(theta_1) - config.l3_z*cos(theta_1)*cos(theta_2) + config.l4_x_com*sin(theta_2)*cos(theta_1) - config.l4_y_com*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l4_z_com*cos(theta_1)*cos(theta_2)*cos(theta_3)) + config.m_ul*(-config.l4_y_com*sin(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z_com*sin(theta_1)*sin(theta_3)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))) + config.m_ul*(-config.l4_y_com*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z_com*sin(theta_3)*cos(theta_1)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))) + ((-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*sin(theta_2) - (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*cos(theta_2))*cos(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*sin(theta_2)*cos(theta_1) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*sin(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2))*sin(theta_2)*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2))*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2))*cos(theta_2) + ((-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_2) - (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*sin(theta_3)*cos(theta_2) - (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*cos(theta_2)*cos(theta_3))*cos(theta_2) - ((-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*sin(theta_1)*cos(theta_2) + (-config.Ixy_ul*sin(theta_2) + config.Izy_ul*cos(theta_2))*cos(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_1)*sin(theta_2))*sin(theta_1)*sin(theta_2) - ((-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*cos(theta_1)*cos(theta_2) - (-config.Ixy_ul*sin(theta_2) + config.Izy_ul*cos(theta_2))*sin(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_2)*cos(theta_1))*sin(theta_2)*cos(theta_1) + (-(config.Ixx_ul*sin(theta_2) - config.Izx_ul*cos(theta_2))*cos(theta_2) - (config.Ixx_ul*cos(theta_2) + config.Izx_ul*sin(theta_2))*sin(theta_2) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_2) - (-config.Ixz_ul*cos(theta_2) - config.Izz_ul*sin(theta_2))*cos(theta_2))*sin(theta_2) + (-(-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*sin(theta_1)*sin(theta_2) + (-config.Ixx_ul*cos(theta_2) - config.Izx_ul*sin(theta_2))*sin(theta_1)*cos(theta_2) + (-config.Ixy_ul*cos(theta_2) - config.Izy_ul*sin(theta_2))*cos(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*sin(theta_1)*cos(theta_2) + (-config.Ixz_ul*cos(theta_2) - config.Izz_ul*sin(theta_2))*sin(theta_1)*sin(theta_2))*sin(theta_1)*cos(theta_2) + (-(-config.Ixx_ul*sin(theta_2) + config.Izx_ul*cos(theta_2))*sin(theta_2)*cos(theta_1) + (-config.Ixx_ul*cos(theta_2) - config.Izx_ul*sin(theta_2))*cos(theta_1)*cos(theta_2) + (config.Ixy_ul*cos(theta_2) + config.Izy_ul*sin(theta_2))*sin(theta_1) + (-config.Ixz_ul*sin(theta_2) + config.Izz_ul*cos(theta_2))*cos(theta_1)*cos(theta_2) + (-config.Ixz_ul*cos(theta_2) - config.Izz_ul*sin(theta_2))*sin(theta_2)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*cos(theta_2) + config.Iyy_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izy_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*cos(theta_2) + config.Iyz_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izz_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) + (-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2)*cos(theta_1) + (-config.Ixx_foot*cos(theta_2) + config.Iyx_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izx_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*cos(theta_2) + config.Iyz_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izz_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*cos(theta_2) + config.Iyy_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izy_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) + (-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-config.Ixx_foot*sin(theta_2) + config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*sin(theta_2) + (-config.Ixx_foot*cos(theta_2) + config.Iyx_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izx_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(-config.Ixz_ll*cos(theta_2) - config.Iyz_ll*sin(theta_2)*sin(theta_3) - config.Izz_ll*sin(theta_2)*cos(theta_3)) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(-config.Ixy_ll*cos(theta_2) - config.Iyy_ll*sin(theta_2)*sin(theta_3) - config.Izy_ll*sin(theta_2)*cos(theta_3)) - (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_2)*cos(theta_1) + (-config.Ixx_ll*cos(theta_2) - config.Iyx_ll*sin(theta_2)*sin(theta_3) - config.Izx_ll*sin(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*sin(theta_3)*cos(theta_1)*cos(theta_2) + (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*cos(theta_2) - config.Iyy_ll*sin(theta_2)*sin(theta_3) - config.Izy_ll*sin(theta_2)*cos(theta_3)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*cos(theta_2) - config.Iyz_ll*sin(theta_2)*sin(theta_3) - config.Izz_ll*sin(theta_2)*cos(theta_3)) - (-config.Ixx_ll*sin(theta_2) + config.Iyx_ll*sin(theta_3)*cos(theta_2) + config.Izx_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*sin(theta_2) + (-config.Ixx_ll*cos(theta_2) - config.Iyx_ll*sin(theta_2)*sin(theta_3) - config.Izx_ll*sin(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*sin(theta_3)*cos(theta_2) + (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixz_foot*cos(theta_2) + config.Iyz_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izz_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixy_foot*cos(theta_2) + config.Iyy_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) + config.Izy_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4))) - (config.Ixx_foot*sin(theta_2) - config.Iyx_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - config.Izx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_2) - (config.Ixx_foot*cos(theta_2) - config.Iyx_foot*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) - config.Izx_foot*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2))*sin(theta_2) + (-(config.Ixx_ll*sin(theta_2) - config.Iyx_ll*sin(theta_3)*cos(theta_2) - config.Izx_ll*cos(theta_2)*cos(theta_3))*cos(theta_2) - (config.Ixx_ll*cos(theta_2) + config.Iyx_ll*sin(theta_2)*sin(theta_3) + config.Izx_ll*sin(theta_2)*cos(theta_3))*sin(theta_2) + (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*sin(theta_2)*sin(theta_3) - (-config.Ixy_ll*cos(theta_2) - config.Iyy_ll*sin(theta_2)*sin(theta_3) - config.Izy_ll*sin(theta_2)*cos(theta_3))*sin(theta_3)*cos(theta_2) + (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*sin(theta_2)*cos(theta_3) - (-config.Ixz_ll*cos(theta_2) - config.Iyz_ll*sin(theta_2)*sin(theta_3) - config.Izz_ll*sin(theta_2)*cos(theta_3))*cos(theta_2)*cos(theta_3))*sin(theta_2))*thetadot1 + (config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_1) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*sin(theta_1) + config.m_foot*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l3_x*sin(theta_2) + config.l3_z*cos(theta_2) - config.l4_x*sin(theta_2) + config.l4_y*sin(theta_3)*cos(theta_2) + config.l4_z*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x_com*sin(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_2) - config.m_foot*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*sin(theta_2) + config.m_foot*(-config.l4_y*sin(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_1)*sin(theta_3)*cos(theta_2) - config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2) - config.l6_y_com*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l4_y*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_1) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*sin(theta_1) + config.m_ll*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(-config.l3_x*sin(theta_2) + config.l3_z*cos(theta_2) - config.l4_x*sin(theta_2) + config.l4_y*sin(theta_3)*cos(theta_2) + config.l4_z*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_2) - config.m_ll*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*sin(theta_2) + config.m_ll*(-config.l4_y*sin(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_1)*sin(theta_3)*cos(theta_2) - config.l5_y*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l4_y*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_1)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3))*cos(theta_1) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3))*sin(theta_1) + config.m_ul*(-config.l4_y_com*cos(theta_3) + config.l4_z_com*sin(theta_3))*(-config.l3_x*sin(theta_2) + config.l3_z*cos(theta_2) - config.l4_x_com*sin(theta_2) + config.l4_y_com*sin(theta_3)*cos(theta_2) + config.l4_z_com*cos(theta_2)*cos(theta_3))*cos(theta_2) - config.m_ul*(-config.l4_y_com*cos(theta_3) + config.l4_z_com*sin(theta_3))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3))*sin(theta_2) + config.m_ul*(-config.l4_y_com*sin(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z_com*sin(theta_1)*sin(theta_3)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*sin(theta_1) + config.m_ul*(-config.l4_y_com*cos(theta_1)*cos(theta_2)*cos(theta_3) + config.l4_z_com*sin(theta_3)*cos(theta_1)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*cos(theta_1) + (config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_2) - config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_2))*cos(theta_2) + (config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_2) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_2))*sin(theta_2) + (-config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1)*sin(theta_2) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + (-config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_2)*cos(theta_1) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*cos(theta_2) + (-(-config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) - config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_2)*sin(theta_3) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_2)*cos(theta_3))*sin(theta_2) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_2) - (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_3)*cos(theta_2) - (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_2)*cos(theta_3))*cos(theta_2) - ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1)*sin(theta_2) - ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*sin(theta_2)*cos(theta_1) + (-(config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*sin(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*sin(theta_3)*cos(theta_2) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)*cos(theta_2) - ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*sin(theta_1)*sin(theta_2) + (-(config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_2)*cos(theta_1) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_3)*cos(theta_1)*cos(theta_2) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_1)*cos(theta_2)*cos(theta_3))*cos(theta_1)*cos(theta_2) - ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*sin(theta_2)*cos(theta_1) - (config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1)*cos(theta_2) + config.Iyy_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1)*sin(theta_2))*sin(theta_1)*sin(theta_2) - (config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1)*cos(theta_2) - config.Iyy_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_2)*cos(theta_1))*sin(theta_2)*cos(theta_1) + (-(-config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_2) - (-config.Iyx_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) - config.Izx_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*sin(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_2)*sin(theta_3)*cos(theta_4) - sin(theta_2)*sin(theta_4)*cos(theta_3)) - (config.Iyy_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izy_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_2)*sin(theta_3)*sin(theta_4) - sin(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Iyz_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izz_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) + (-(config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*sin(theta_2) + (config.Iyx_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izx_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyy_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izy_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4)) + (config.Iyz_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izz_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1)*cos(theta_2) + (-(config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_2)*cos(theta_1) + (config.Iyx_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izx_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3)) + (config.Iyy_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izy_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) + cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4)) + (config.Iyz_foot*((sin(theta_1)*sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_1)*sin(theta_4)*cos(theta_2)*cos(theta_3))*cos(theta_1) + (-sin(theta_3)*cos(theta_1)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_1)*cos(theta_2)*cos(theta_3))*sin(theta_1)) + config.Izz_foot*((-sin(theta_1)*sin(theta_3)*sin(theta_4)*cos(theta_2) + sin(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*cos(theta_1) + (sin(theta_3)*sin(theta_4)*cos(theta_1)*cos(theta_2) - cos(theta_1)*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1)*cos(theta_2))*thetadot2)*thetadot3 + (-config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*thetadot1*sin(theta_1) + config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*thetadot2*cos(theta_2) - config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*thetadot2*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*thetadot1*cos(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*thetadot2*cos(theta_2) - config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*thetadot2*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*sin(theta_1) + config.m_foot*(config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) - config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*thetadot2*sin(theta_2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l3_x*thetadot2*sin(theta_2) + config.l3_z*thetadot2*cos(theta_2) - config.l4_x*thetadot2*sin(theta_2) + config.l4_y*thetadot2*sin(theta_3)*cos(theta_2) + config.l4_y*thetadot3*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot2*cos(theta_2)*cos(theta_3) - config.l4_z*thetadot3*sin(theta_2)*sin(theta_3) - config.l5_x*thetadot2*sin(theta_2) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*thetadot2*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x_com*thetadot2*sin(theta_2) + config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_2) + config.m_foot*(config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot2*cos(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*thetadot1*cos(theta_1)*cos(theta_2) - ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*thetadot2*sin(theta_1)*sin(theta_2) - ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*thetadot1*sin(theta_1)*cos(theta_2) - ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*thetadot2*sin(theta_2)*cos(theta_1) + (-(-config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*thetadot2*cos(theta_2) - (-config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) - config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) + (-(config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*thetadot1*sin(theta_1)*cos(theta_2) - (config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*thetadot2*sin(theta_2)*cos(theta_1) + (config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1)*cos(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1)*cos(theta_2))*thetadot5 + (config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(2*config.l3_x*thetadot2*cos(theta_2) + 2*config.l3_z*thetadot2*sin(theta_2) + 2*config.l4_x*thetadot2*cos(theta_2) + 2*config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - 2*config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + 2*config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + 2*config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + 2*config.l5_x*thetadot2*cos(theta_2) - 2*config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + 2*config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4) + 2*config.l6_x_com*thetadot2*cos(theta_2) - 2*config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + 2*config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + 2*config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + 2*config.l6_z_com*thetadot2*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*pow(sin(theta_1), 2) + config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(2*config.l3_x*thetadot2*cos(theta_2) + 2*config.l3_z*thetadot2*sin(theta_2) + 2*config.l4_x*thetadot2*cos(theta_2) + 2*config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - 2*config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + 2*config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + 2*config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + 2*config.l5_x*thetadot2*cos(theta_2) - 2*config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + 2*config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4) + 2*config.l6_x_com*thetadot2*cos(theta_2) - 2*config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + 2*config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + 2*config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + 2*config.l6_z_com*thetadot2*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*pow(cos(theta_1), 2) + config.m_foot*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*(-2*config.l3_x*thetadot2*sin(theta_2) + 2*config.l3_z*thetadot2*cos(theta_2) - 2*config.l4_x*thetadot2*sin(theta_2) + 2*config.l4_y*thetadot2*sin(theta_3)*cos(theta_2) + 2*config.l4_y*thetadot3*sin(theta_2)*cos(theta_3) + 2*config.l4_z*thetadot2*cos(theta_2)*cos(theta_3) - 2*config.l4_z*thetadot3*sin(theta_2)*sin(theta_3) - 2*config.l5_x*thetadot2*sin(theta_2) + 2*config.l5_y*(thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4) + 2*config.l5_y*thetadot2*sin(theta_3 + theta_4)*cos(theta_2) - 2*config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2) + 2*config.l5_z*thetadot2*cos(theta_3 + theta_4)*cos(theta_2) - 2*config.l6_x_com*thetadot2*sin(theta_2) + 2*config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + 2*config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - 2*config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + 2*config.l6_z_com*thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(2*config.l3_x_com*thetadot2*cos(theta_2) + 2*config.l3_z_com*thetadot2*sin(theta_2))*pow(sin(theta_1), 2) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(2*config.l3_x_com*thetadot2*cos(theta_2) + 2*config.l3_z_com*thetadot2*sin(theta_2))*pow(cos(theta_1), 2) + config.m_hip1*(config.l3_x_com*cos(theta_2) + config.l3_z_com*sin(theta_2))*(-2*config.l3_x_com*thetadot2*sin(theta_2) + 2*config.l3_z_com*thetadot2*cos(theta_2)) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(2*config.l3_x*thetadot2*cos(theta_2) + 2*config.l3_z*thetadot2*sin(theta_2) + 2*config.l4_x*thetadot2*cos(theta_2) + 2*config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - 2*config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + 2*config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + 2*config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + 2*config.l5_x*thetadot2*cos(theta_2) - 2*config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + 2*config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4))*pow(sin(theta_1), 2) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(2*config.l3_x*thetadot2*cos(theta_2) + 2*config.l3_z*thetadot2*sin(theta_2) + 2*config.l4_x*thetadot2*cos(theta_2) + 2*config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - 2*config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + 2*config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + 2*config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + 2*config.l5_x*thetadot2*cos(theta_2) - 2*config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + 2*config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4))*pow(cos(theta_1), 2) + config.m_ll*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*(-2*config.l3_x*thetadot2*sin(theta_2) + 2*config.l3_z*thetadot2*cos(theta_2) - 2*config.l4_x*thetadot2*sin(theta_2) + 2*config.l4_y*thetadot2*sin(theta_3)*cos(theta_2) + 2*config.l4_y*thetadot3*sin(theta_2)*cos(theta_3) + 2*config.l4_z*thetadot2*cos(theta_2)*cos(theta_3) - 2*config.l4_z*thetadot3*sin(theta_2)*sin(theta_3) - 2*config.l5_x*thetadot2*sin(theta_2) + 2*config.l5_y*(thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4) + 2*config.l5_y*thetadot2*sin(theta_3 + theta_4)*cos(theta_2) - 2*config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2) + 2*config.l5_z*thetadot2*cos(theta_3 + theta_4)*cos(theta_2)) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(2*config.l3_x*thetadot2*cos(theta_2) + 2*config.l3_z*thetadot2*sin(theta_2) + 2*config.l4_x_com*thetadot2*cos(theta_2) + 2*config.l4_y_com*thetadot2*sin(theta_2)*sin(theta_3) - 2*config.l4_y_com*thetadot3*cos(theta_2)*cos(theta_3) + 2*config.l4_z_com*thetadot2*sin(theta_2)*cos(theta_3) + 2*config.l4_z_com*thetadot3*sin(theta_3)*cos(theta_2))*pow(sin(theta_1), 2) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(2*config.l3_x*thetadot2*cos(theta_2) + 2*config.l3_z*thetadot2*sin(theta_2) + 2*config.l4_x_com*thetadot2*cos(theta_2) + 2*config.l4_y_com*thetadot2*sin(theta_2)*sin(theta_3) - 2*config.l4_y_com*thetadot3*cos(theta_2)*cos(theta_3) + 2*config.l4_z_com*thetadot2*sin(theta_2)*cos(theta_3) + 2*config.l4_z_com*thetadot3*sin(theta_3)*cos(theta_2))*pow(cos(theta_1), 2) + config.m_ul*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3))*(-2*config.l3_x*thetadot2*sin(theta_2) + 2*config.l3_z*thetadot2*cos(theta_2) - 2*config.l4_x_com*thetadot2*sin(theta_2) + 2*config.l4_y_com*thetadot2*sin(theta_3)*cos(theta_2) + 2*config.l4_y_com*thetadot3*sin(theta_2)*cos(theta_3) + 2*config.l4_z_com*thetadot2*cos(theta_2)*cos(theta_3) - 2*config.l4_z_com*thetadot3*sin(theta_2)*sin(theta_3)) - (config.Iyx_hip1*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1) + config.Iyy_hip1*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1))*thetadot1*sin(theta_1) + (-config.Iyx_hip1*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1) + config.Iyy_hip1*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1))*thetadot1*cos(theta_1) + (config.Iyx_hip1*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot1*sin(theta_1) + config.Iyy_hip1*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot1*cos(theta_1))*sin(theta_1) + (config.Iyx_hip1*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot1*cos(theta_1) - config.Iyy_hip1*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot1*sin(theta_1))*cos(theta_1) - ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*thetadot1*sin(theta_1) + (-(config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*thetadot1*cos(theta_1) - ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*thetadot1*sin(theta_1) + (-(config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_1)*cos(theta_2) - (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) - (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*thetadot1*cos(theta_1) - (config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1)*cos(theta_2) + config.Iyy_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1)*sin(theta_2))*thetadot1*sin(theta_1) + (-config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1)*cos(theta_2) + config.Iyy_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1) - config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_2)*cos(theta_1))*thetadot1*cos(theta_1) + (config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot1*sin(theta_1)*cos(theta_2) + config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot2*sin(theta_2)*cos(theta_1) + config.Iyy_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot1*cos(theta_1) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot1*sin(theta_1)*sin(theta_2) - config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot2*cos(theta_1)*cos(theta_2))*sin(theta_1) + (config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot1*cos(theta_1)*cos(theta_2) - config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot2*sin(theta_1)*sin(theta_2) - config.Iyy_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot1*sin(theta_1) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot1*sin(theta_2)*cos(theta_1) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot2*sin(theta_1)*cos(theta_2))*cos(theta_1) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*thetadot1*sin(theta_1)*cos(theta_2) + (config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*thetadot2*sin(theta_2)*cos(theta_1) - (config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) - (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*sin(theta_1) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*cos(theta_1) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot1*sin(theta_1)*cos(theta_2) + (config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot2*sin(theta_2)*cos(theta_1) - (config.Iyx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*cos(theta_1)*cos(theta_2) - (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - (config.Iyy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) - (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - (config.Iyz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*sin(theta_1) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Iyx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + (config.Iyy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*cos(theta_1))*thetadot2 + (-config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*thetadot1*sin(theta_1) + config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*thetadot2*cos(theta_2) - config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*thetadot2*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_1) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*thetadot1*cos(theta_1) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*thetadot2*cos(theta_2) - config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*thetadot2*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*sin(theta_1) + config.m_foot*(config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) + config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) - config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*thetadot2*sin(theta_2) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l3_x*thetadot2*sin(theta_2) + config.l3_z*thetadot2*cos(theta_2) - config.l4_x*thetadot2*sin(theta_2) + config.l4_y*thetadot2*sin(theta_3)*cos(theta_2) + config.l4_y*thetadot3*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot2*cos(theta_2)*cos(theta_3) - config.l4_z*thetadot3*sin(theta_2)*sin(theta_3) - config.l5_x*thetadot2*sin(theta_2) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*thetadot2*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x_com*thetadot2*sin(theta_2) + config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_2) + config.m_foot*(config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4) + config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) - config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*thetadot1*sin(theta_1) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_1) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*thetadot1*cos(theta_1) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4))*sin(theta_1) + config.m_ll*(config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) - config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*thetadot2*sin(theta_2) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(-config.l3_x*thetadot2*sin(theta_2) + config.l3_z*thetadot2*cos(theta_2) - config.l4_x*thetadot2*sin(theta_2) + config.l4_y*thetadot2*sin(theta_3)*cos(theta_2) + config.l4_y*thetadot3*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot2*cos(theta_2)*cos(theta_3) - config.l4_z*thetadot3*sin(theta_2)*sin(theta_3) - config.l5_x*thetadot2*sin(theta_2) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*thetadot2*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_2) + config.m_ll*(config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot2*cos(theta_2) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_2) - (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_3)*cos(theta_2) - (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*thetadot1*cos(theta_1)*cos(theta_2) - ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*thetadot2*sin(theta_1)*sin(theta_2) - ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*thetadot1*sin(theta_1)*cos(theta_2) - ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*thetadot2*sin(theta_2)*cos(theta_1) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*thetadot1*cos(theta_1)*cos(theta_2) - ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*thetadot2*sin(theta_1)*sin(theta_2) - ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*thetadot1*sin(theta_1)*cos(theta_2) - ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*thetadot2*sin(theta_2)*cos(theta_1) + (-(-config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*thetadot2*cos(theta_2) - (-config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) - config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) + (-(config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*thetadot1*sin(theta_1)*cos(theta_2) - (config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*thetadot2*sin(theta_2)*cos(theta_1) + (config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1)*cos(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1)*cos(theta_2) + (-(config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot1*sin(theta_1)*cos(theta_2) - (config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot2*sin(theta_2)*cos(theta_1) + (config.Iyx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) + (config.Iyy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*cos(theta_1)*cos(theta_2) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Iyx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + (config.Iyy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (-(-config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) - config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot2*cos(theta_2) - (-config.Iyx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) - config.Izx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*sin(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot2*sin(theta_2)*sin(theta_3) - (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot3*cos(theta_2)*cos(theta_3) - (config.Iyy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*sin(theta_3)*cos(theta_2) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot2*sin(theta_2)*cos(theta_3) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot3*sin(theta_3)*cos(theta_2) - (config.Iyz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*cos(theta_2)*cos(theta_3))*sin(theta_2))*thetadot4 + (-config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot2*cos(theta_2) - config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot2*sin(theta_2) - config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*thetadot1*sin(theta_1) + config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*thetadot1*cos(theta_1) + config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l2_x*thetadot1*sin(theta_1) + config.l2_y*thetadot1*cos(theta_1) + config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l3_y*thetadot1*cos(theta_1) + config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + config.l4_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l4_y*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l5_x*thetadot2*sin(theta_2)*cos(theta_1) - config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l6_x_com*thetadot1*sin(theta_1)*cos(theta_2) + config.l6_x_com*thetadot2*sin(theta_2)*cos(theta_1) - config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) - config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)))*sin(theta_1) + config.m_foot*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l2_x*thetadot1*cos(theta_1) - config.l2_y*thetadot1*sin(theta_1) + config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - config.l3_y*thetadot1*sin(theta_1) + config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l4_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l4_y*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l4_z*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l5_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l5_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l6_x_com*thetadot1*cos(theta_1)*cos(theta_2) - config.l6_x_com*thetadot2*sin(theta_1)*sin(theta_2) + config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)) - config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)))*cos(theta_1) + config.m_foot*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*thetadot2*cos(theta_2) - config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*thetadot2*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_1) + config.m_foot*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*thetadot2*cos(theta_2) - config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*thetadot2*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*sin(theta_1) - config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x_com*sin(theta_1)*cos(theta_2) + config.l3_y_com*cos(theta_1) + config.l3_z_com*sin(theta_1)*sin(theta_2))*thetadot1*sin(theta_1) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x_com*cos(theta_1)*cos(theta_2) + config.l3_y_com*sin(theta_1) - config.l3_z_com*sin(theta_2)*cos(theta_1))*thetadot1*cos(theta_1) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(config.l2_x*thetadot1*sin(theta_1) + config.l2_y*thetadot1*cos(theta_1) + config.l3_x_com*thetadot1*sin(theta_1)*cos(theta_2) + config.l3_x_com*thetadot2*sin(theta_2)*cos(theta_1) + config.l3_y_com*thetadot1*cos(theta_1) + config.l3_z_com*thetadot1*sin(theta_1)*sin(theta_2) - config.l3_z_com*thetadot2*cos(theta_1)*cos(theta_2))*sin(theta_1) + config.m_hip1*(config.l3_x_com*sin(theta_2) - config.l3_z_com*cos(theta_2))*(config.l2_x*thetadot1*cos(theta_1) - config.l2_y*thetadot1*sin(theta_1) + config.l3_x_com*thetadot1*cos(theta_1)*cos(theta_2) - config.l3_x_com*thetadot2*sin(theta_1)*sin(theta_2) - config.l3_y_com*thetadot1*sin(theta_1) + config.l3_z_com*thetadot1*sin(theta_2)*cos(theta_1) + config.l3_z_com*thetadot2*sin(theta_1)*cos(theta_2))*cos(theta_1) + config.m_hip1*(config.l3_x_com*thetadot2*cos(theta_2) + config.l3_z_com*thetadot2*sin(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x_com*sin(theta_1)*cos(theta_2) + config.l3_y_com*cos(theta_1) + config.l3_z_com*sin(theta_1)*sin(theta_2))*cos(theta_1) + config.m_hip1*(config.l3_x_com*thetadot2*cos(theta_2) + config.l3_z_com*thetadot2*sin(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x_com*cos(theta_1)*cos(theta_2) + config.l3_y_com*sin(theta_1) - config.l3_z_com*sin(theta_2)*cos(theta_1))*sin(theta_1) - config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*thetadot1*sin(theta_1) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*thetadot1*cos(theta_1) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(config.l2_x*thetadot1*sin(theta_1) + config.l2_y*thetadot1*cos(theta_1) + config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l3_y*thetadot1*cos(theta_1) + config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + config.l4_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l4_y*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l5_x*thetadot2*sin(theta_2)*cos(theta_1) - config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)))*sin(theta_1) + config.m_ll*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*(config.l2_x*thetadot1*cos(theta_1) - config.l2_y*thetadot1*sin(theta_1) + config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - config.l3_y*thetadot1*sin(theta_1) + config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l4_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l4_y*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l4_z*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l5_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l5_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)))*cos(theta_1) + config.m_ll*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_1) + config.m_ll*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4))*sin(theta_1) - config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*thetadot1*sin(theta_1) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*thetadot1*cos(theta_1) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(config.l2_x*thetadot1*sin(theta_1) + config.l2_y*thetadot1*cos(theta_1) + config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l3_y*thetadot1*cos(theta_1) + config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + config.l4_x_com*thetadot1*sin(theta_1)*cos(theta_2) + config.l4_x_com*thetadot2*sin(theta_2)*cos(theta_1) + config.l4_y_com*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)))*sin(theta_1) + config.m_ul*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*(config.l2_x*thetadot1*cos(theta_1) - config.l2_y*thetadot1*sin(theta_1) + config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - config.l3_y*thetadot1*sin(theta_1) + config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + config.l4_x_com*thetadot1*cos(theta_1)*cos(theta_2) - config.l4_x_com*thetadot2*sin(theta_1)*sin(theta_2) + config.l4_y_com*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l4_z_com*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)))*cos(theta_1) + config.m_ul*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x_com*thetadot2*cos(theta_2) + config.l4_y_com*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y_com*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z_com*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z_com*thetadot3*sin(theta_3)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*cos(theta_1) + config.m_ul*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x_com*thetadot2*cos(theta_2) + config.l4_y_com*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y_com*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z_com*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z_com*thetadot3*sin(theta_3)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*sin(theta_1) + (-config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*thetadot2*cos(theta_2) + (-config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) - config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_2) + (-config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) - config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot2*cos(theta_2) + (-config.Iyx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) - config.Izx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*sin(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot2*sin(theta_2)*sin(theta_3) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot3*cos(theta_2)*cos(theta_3) + (config.Iyy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*sin(theta_3)*cos(theta_2) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot2*sin(theta_2)*cos(theta_3) - (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot3*sin(theta_3)*cos(theta_2) + (config.Iyz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*cos(theta_2)*cos(theta_3))*thetadot1 + (-config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*thetadot1*sin(theta_1) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*thetadot2*cos(theta_2) - config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*thetadot2*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_1) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*thetadot1*cos(theta_1) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*thetadot2*cos(theta_2) - config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*thetadot2*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*sin(theta_1) + config.m_foot*(-config.l4_y*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l4_z*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) + config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l4_y*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) - config.m_foot*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*thetadot2*sin(theta_2) + config.m_foot*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l3_x*thetadot2*sin(theta_2) + config.l3_z*thetadot2*cos(theta_2) - config.l4_x*thetadot2*sin(theta_2) + config.l4_y*thetadot2*sin(theta_3)*cos(theta_2) + config.l4_y*thetadot3*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot2*cos(theta_2)*cos(theta_3) - config.l4_z*thetadot3*sin(theta_2)*sin(theta_3) - config.l5_x*thetadot2*sin(theta_2) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*thetadot2*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x_com*thetadot2*sin(theta_2) + config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_2) + config.m_foot*(config.l4_y*thetadot3*sin(theta_3) + config.l4_z*thetadot3*cos(theta_3) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4) + config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) - config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*thetadot1*sin(theta_1) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_1) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*thetadot1*cos(theta_1) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4))*sin(theta_1) + config.m_ll*(-config.l4_y*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l4_z*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l4_y*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) - config.m_ll*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*thetadot2*sin(theta_2) + config.m_ll*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(-config.l3_x*thetadot2*sin(theta_2) + config.l3_z*thetadot2*cos(theta_2) - config.l4_x*thetadot2*sin(theta_2) + config.l4_y*thetadot2*sin(theta_3)*cos(theta_2) + config.l4_y*thetadot3*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot2*cos(theta_2)*cos(theta_3) - config.l4_z*thetadot3*sin(theta_2)*sin(theta_3) - config.l5_x*thetadot2*sin(theta_2) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*thetadot2*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_2) + config.m_ll*(config.l4_y*thetadot3*sin(theta_3) + config.l4_z*thetadot3*cos(theta_3) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_2) - config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*thetadot1*sin(theta_1) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x_com*thetadot2*cos(theta_2) + config.l4_y_com*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y_com*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z_com*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z_com*thetadot3*sin(theta_3)*cos(theta_2))*cos(theta_1) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*thetadot1*cos(theta_1) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x_com*thetadot2*cos(theta_2) + config.l4_y_com*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y_com*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z_com*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z_com*thetadot3*sin(theta_3)*cos(theta_2))*sin(theta_1) + config.m_ul*(-config.l4_y_com*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l4_z_com*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*sin(theta_1) + config.m_ul*(-config.l4_y_com*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*cos(theta_1) - config.m_ul*(-config.l4_y_com*cos(theta_3) + config.l4_z_com*sin(theta_3))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3))*thetadot2*sin(theta_2) + config.m_ul*(-config.l4_y_com*cos(theta_3) + config.l4_z_com*sin(theta_3))*(-config.l3_x*thetadot2*sin(theta_2) + config.l3_z*thetadot2*cos(theta_2) - config.l4_x_com*thetadot2*sin(theta_2) + config.l4_y_com*thetadot2*sin(theta_3)*cos(theta_2) + config.l4_y_com*thetadot3*sin(theta_2)*cos(theta_3) + config.l4_z_com*thetadot2*cos(theta_2)*cos(theta_3) - config.l4_z_com*thetadot3*sin(theta_2)*sin(theta_3))*cos(theta_2) + config.m_ul*(config.l4_y_com*thetadot3*sin(theta_3) + config.l4_z_com*thetadot3*cos(theta_3))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3))*cos(theta_2) + (config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_2) - config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_2))*thetadot2*cos(theta_2) + (config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot2*cos(theta_2) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot2*sin(theta_2))*sin(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot2*cos(theta_2) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_2) - (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_3)*cos(theta_2) - (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*thetadot1*cos(theta_1)*cos(theta_2) - ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*thetadot2*sin(theta_1)*sin(theta_2) - ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*thetadot1*sin(theta_1)*cos(theta_2) - ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*thetadot2*sin(theta_2)*cos(theta_1) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*thetadot1*cos(theta_1)*cos(theta_2) - ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*thetadot2*sin(theta_1)*sin(theta_2) - ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*thetadot1*sin(theta_1)*cos(theta_2) - ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*thetadot2*sin(theta_2)*cos(theta_1) + (config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1)*cos(theta_2) + config.Iyy_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1)*sin(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1)*cos(theta_2) + config.Iyy_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1)*sin(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) - (config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1)*cos(theta_2) - config.Iyy_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_2)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*cos(theta_1)*cos(theta_2) - config.Iyy_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_1) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*sin(theta_2)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (-config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot1*sin(theta_1)*cos(theta_2) - config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot2*sin(theta_2)*cos(theta_1) - config.Iyy_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot1*cos(theta_1) - config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot1*sin(theta_1)*sin(theta_2) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot2*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + (config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot1*cos(theta_1)*cos(theta_2) - config.Iyx_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot2*sin(theta_1)*sin(theta_2) - config.Iyy_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot1*sin(theta_1) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot1*sin(theta_2)*cos(theta_1) + config.Iyz_ul*(pow(sin(theta_1), 2) + pow(cos(theta_1), 2))*thetadot2*sin(theta_1)*cos(theta_2))*sin(theta_1)*cos(theta_2) + (-(-config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*thetadot2*cos(theta_2) - (-config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) - config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) + (-(config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*thetadot1*sin(theta_1)*cos(theta_2) - (config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*thetadot2*sin(theta_2)*cos(theta_1) + (config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1)*cos(theta_2) + ((config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) - (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1)*cos(theta_2) + (-(config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot1*sin(theta_1)*cos(theta_2) - (config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot2*sin(theta_2)*cos(theta_1) + (config.Iyx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) + (config.Iyy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*cos(theta_1)*cos(theta_2) + ((config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Iyx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + (config.Iyy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (-(-config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) - config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot2*cos(theta_2) - (-config.Iyx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) - config.Izx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*sin(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot2*sin(theta_2)*sin(theta_3) - (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot3*cos(theta_2)*cos(theta_3) - (config.Iyy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*sin(theta_3)*cos(theta_2) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot2*sin(theta_2)*cos(theta_3) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*thetadot3*sin(theta_3)*cos(theta_2) - (config.Iyz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1)) + config.Izz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_1) + (thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot1*sin(theta_3)*cos(theta_1) - thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1)))*cos(theta_2)*cos(theta_3))*sin(theta_2))*thetadot3;

    double temp_3 = -0.5L*((config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-2*config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + 2*config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(2*config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + 2*config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(2*config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + 2*config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + (-(-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + ((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1)*cos(theta_2))*thetadot5 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l4_y*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + (-(-config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_2) - (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*sin(theta_2) + ((config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + ((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*cos(theta_1)*cos(theta_2))*thetadot1 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + (-(-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + ((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1)*cos(theta_2))*thetadot4 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l4_y*sin(theta_3) + config.l4_z*cos(theta_3) + config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + (-(-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + ((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1)*cos(theta_2))*thetadot3 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l4_y*sin(theta_2)*cos(theta_3) - config.l4_z*sin(theta_2)*sin(theta_3) + config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4)*sin(theta_2) + config.l6_y_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) - config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2))*cos(theta_2) + (-(-config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) - config.Izx_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) + ((config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1)*cos(theta_2))*thetadot2)*thetadot5 - 0.5L*((config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l4_y*sin(theta_2)*cos(theta_3) - config.l4_z*sin(theta_2)*sin(theta_3) + config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4)*sin(theta_2) + config.l6_y_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) - config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2))*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1) + (-(config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) - ((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1))*thetadot5 + (config.m_foot*(2*config.l4_y*sin(theta_2)*cos(theta_3) - 2*config.l4_z*sin(theta_2)*sin(theta_3) + 2*config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - 2*config.l5_z*sin(theta_3 + theta_4)*sin(theta_2) + 2*config.l6_y_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) - 2*config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.m_foot*(-2*config.l4_y*cos(theta_2)*cos(theta_3) + 2*config.l4_z*sin(theta_3)*cos(theta_2) - 2*config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - 2*config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + 2*config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*pow(sin(theta_1), 2) + config.m_foot*(-2*config.l4_y*cos(theta_2)*cos(theta_3) + 2*config.l4_z*sin(theta_3)*cos(theta_2) - 2*config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - 2*config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + 2*config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*pow(cos(theta_1), 2) + config.m_ll*(2*config.l4_y*sin(theta_2)*cos(theta_3) - 2*config.l4_z*sin(theta_2)*sin(theta_3) + 2*config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - 2*config.l5_z*sin(theta_3 + theta_4)*sin(theta_2))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4)) + config.m_ll*(-2*config.l4_y*cos(theta_2)*cos(theta_3) + 2*config.l4_z*sin(theta_3)*cos(theta_2) - 2*config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*pow(sin(theta_1), 2) + config.m_ll*(-2*config.l4_y*cos(theta_2)*cos(theta_3) + 2*config.l4_z*sin(theta_3)*cos(theta_2) - 2*config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*pow(cos(theta_1), 2) + config.m_ul*(2*config.l4_y_com*sin(theta_2)*cos(theta_3) - 2*config.l4_z_com*sin(theta_2)*sin(theta_3))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3)) + config.m_ul*(-2*config.l4_y_com*cos(theta_2)*cos(theta_3) + 2*config.l4_z_com*sin(theta_3)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*pow(sin(theta_1), 2) + config.m_ul*(-2*config.l4_y_com*cos(theta_2)*cos(theta_3) + 2*config.l4_z_com*sin(theta_3)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*pow(cos(theta_1), 2) + ((config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*cos(theta_1) + (-(config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) - (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*sin(theta_1) + ((config.Iyx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)))*cos(theta_1) + (-(config.Iyx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*cos(theta_1)*cos(theta_2) - (config.Iyy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) - (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - (config.Iyz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*sin(theta_1))*thetadot2 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l4_y*sin(theta_2)*cos(theta_3) - config.l4_z*sin(theta_2)*sin(theta_3) + config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4)*sin(theta_2) + config.l6_y_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) - config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2))*cos(theta_2) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_2) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l4_y*sin(theta_2)*cos(theta_3) - config.l4_z*sin(theta_2)*sin(theta_3) + config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4)*sin(theta_2))*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1) + (-(config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) - ((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1) + ((config.Iyx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))))*cos(theta_1) + (-(config.Iyx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Iyy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) - (config.Iyz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))))*sin(theta_1))*thetadot4 + (config.m_foot*(config.l4_y*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*cos(theta_1) + config.m_foot*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*sin(theta_1) + config.m_ll*(config.l4_y*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*cos(theta_1) + config.m_ll*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*sin(theta_1) + config.m_ul*(config.l4_y_com*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*sin(theta_1) + config.m_ul*(config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*cos(theta_1) + config.m_ul*(-config.l4_y_com*cos(theta_2)*cos(theta_3) + config.l4_z_com*sin(theta_3)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*cos(theta_1) + config.m_ul*(-config.l4_y_com*cos(theta_2)*cos(theta_3) + config.l4_z_com*sin(theta_3)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Iyz_ll*cos(theta_2)*cos(theta_3) - config.Izz_ll*sin(theta_3)*cos(theta_2)) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Iyy_ll*cos(theta_2)*cos(theta_3) - config.Izy_ll*sin(theta_3)*cos(theta_2)) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) - (config.Iyx_ll*cos(theta_2)*cos(theta_3) - config.Izx_ll*sin(theta_3)*cos(theta_2))*cos(theta_1)*cos(theta_2))*sin(theta_1) + ((config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*cos(theta_1) + (-(config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2) - (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) - ((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*sin(theta_1) + ((config.Iyx_ll*cos(theta_2)*cos(theta_3) - config.Izx_ll*sin(theta_3)*cos(theta_2))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*cos(theta_2)*cos(theta_3) - config.Izy_ll*sin(theta_3)*cos(theta_2))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*cos(theta_2)*cos(theta_3) - config.Izz_ll*sin(theta_3)*cos(theta_2))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)))*cos(theta_1))*thetadot1 + (config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(-config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(config.l4_y*sin(theta_3) + config.l4_z*cos(theta_3) + config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l4_y*sin(theta_2)*cos(theta_3) - config.l4_z*sin(theta_2)*sin(theta_3) + config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4)*sin(theta_2) + config.l6_y_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) - config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2))*cos(theta_2) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(-config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(config.l4_y*sin(theta_3) + config.l4_z*cos(theta_3) + config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_2) + config.m_ll*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l4_y*sin(theta_2)*cos(theta_3) - config.l4_z*sin(theta_2)*sin(theta_3) + config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4)*sin(theta_2))*cos(theta_2) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(-config.l4_y_com*cos(theta_2)*cos(theta_3) + config.l4_z_com*sin(theta_3)*cos(theta_2))*cos(theta_1) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*cos(theta_1) + config.m_ul*(-config.l4_y_com*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*sin(theta_1) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(-config.l4_y_com*cos(theta_2)*cos(theta_3) + config.l4_z_com*sin(theta_3)*cos(theta_2))*sin(theta_1) + config.m_ul*(config.l4_y_com*sin(theta_3) + config.l4_z_com*cos(theta_3))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3))*cos(theta_2) + config.m_ul*(-config.l4_y_com*cos(theta_3) + config.l4_z_com*sin(theta_3))*(config.l4_y_com*sin(theta_2)*cos(theta_3) - config.l4_z_com*sin(theta_2)*sin(theta_3))*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1) + (-(config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) - ((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1) + ((config.Iyx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))))*cos(theta_1) + (-(config.Iyx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Iyy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) - (config.Iyz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))))*sin(theta_1))*thetadot3)*thetadot2 - 0.5L*((config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + (-(-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + ((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1)*cos(theta_2))*thetadot5 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-2*config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + 2*config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - 2*config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + 2*config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(2*config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + 2*config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + 2*config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + 2*config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(2*config.l5_y*sin(theta_3 + theta_4) + 2*config.l5_z*cos(theta_3 + theta_4) + 2*config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + 2*config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-2*config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + 2*config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(2*config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + 2*config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ll*(2*config.l5_y*sin(theta_3 + theta_4) + 2*config.l5_z*cos(theta_3 + theta_4))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*pow(cos(theta_2), 2) + (-(-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_2) + (-(-config.Iyx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) - config.Izx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_2) - (config.Iyy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_3)*cos(theta_2) - (config.Iyz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_2)*cos(theta_3) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2)*cos(theta_3) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_2))*sin(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + ((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1)*cos(theta_2) + ((config.Iyx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))))*cos(theta_1)*cos(theta_2))*thetadot4 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l4_y*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l4_y*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + (-(-config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_2) - (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*sin(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Iyz_ll*cos(theta_2)*cos(theta_3) - config.Izz_ll*sin(theta_3)*cos(theta_2)) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Iyy_ll*cos(theta_2)*cos(theta_3) - config.Izy_ll*sin(theta_3)*cos(theta_2)) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (config.Iyx_ll*cos(theta_2)*cos(theta_3) - config.Izx_ll*sin(theta_3)*cos(theta_2))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + (-(-config.Iyx_ll*cos(theta_2)*cos(theta_3) + config.Izx_ll*sin(theta_3)*cos(theta_2))*sin(theta_2) - (config.Iyy_ll*cos(theta_2)*cos(theta_3) - config.Izy_ll*sin(theta_3)*cos(theta_2))*sin(theta_3)*cos(theta_2) - (config.Iyz_ll*cos(theta_2)*cos(theta_3) - config.Izz_ll*sin(theta_3)*cos(theta_2))*cos(theta_2)*cos(theta_3) - (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*cos(theta_2)*cos(theta_3) + (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*sin(theta_3)*cos(theta_2))*sin(theta_2) + ((config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + ((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*cos(theta_1)*cos(theta_2) + ((config.Iyx_ll*cos(theta_2)*cos(theta_3) - config.Izx_ll*sin(theta_3)*cos(theta_2))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*cos(theta_2)*cos(theta_3) - config.Izy_ll*sin(theta_3)*cos(theta_2))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*cos(theta_2)*cos(theta_3) - config.Izz_ll*sin(theta_3)*cos(theta_2))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*thetadot1 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l4_y*sin(theta_3) + config.l4_z*cos(theta_3) + config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4))) + config.m_ll*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*pow(cos(theta_2), 2) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l4_y*sin(theta_3) + config.l4_z*cos(theta_3) + config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4))*pow(cos(theta_2), 2) + (-(-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_2) + (-(-config.Iyx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) - config.Izx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_2) - (config.Iyy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_3)*cos(theta_2) - (config.Iyz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_2)*cos(theta_3) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2)*cos(theta_3) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_2))*sin(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + ((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1)*cos(theta_2) + ((config.Iyx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))))*cos(theta_1)*cos(theta_2))*thetadot3 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l4_y*sin(theta_2)*cos(theta_3) - config.l4_z*sin(theta_2)*sin(theta_3) + config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4)*sin(theta_2) + config.l6_y_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) - config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2))*cos(theta_2) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_2) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l4_y*sin(theta_2)*cos(theta_3) - config.l4_z*sin(theta_2)*sin(theta_3) + config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4)*sin(theta_2))*cos(theta_2) + (-(-config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) - config.Izx_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) + (-(-config.Iyx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) - config.Izx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*sin(theta_2) - (config.Iyy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*sin(theta_3)*cos(theta_2) - (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_2)*cos(theta_3) - (config.Iyz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*cos(theta_2)*cos(theta_3) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_3)*cos(theta_2))*sin(theta_2) + ((config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1)*cos(theta_2) + ((config.Iyx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + ((config.Iyx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*cos(theta_1)*cos(theta_2))*thetadot2)*thetadot4 - 0.5L*((config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l4_y*sin(theta_3) + config.l4_z*cos(theta_3) + config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + (-(-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + ((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1)*cos(theta_2))*thetadot5 + (config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-2*config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - 2*config.l4_z*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - 2*config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + 2*config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - 2*config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + 2*config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-2*config.l4_y*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + 2*config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + 2*config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + 2*config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + 2*config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + 2*config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(2*config.l4_y*sin(theta_3) + 2*config.l4_z*cos(theta_3) + 2*config.l5_y*sin(theta_3 + theta_4) + 2*config.l5_z*cos(theta_3 + theta_4) + 2*config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + 2*config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-2*config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - 2*config.l4_z*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - 2*config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + 2*config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-2*config.l4_y*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + 2*config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + 2*config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + 2*config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(2*config.l4_y*sin(theta_3) + 2*config.l4_z*cos(theta_3) + 2*config.l5_y*sin(theta_3 + theta_4) + 2*config.l5_z*cos(theta_3 + theta_4))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*pow(cos(theta_2), 2) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(-2*config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - 2*config.l4_z_com*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))) + config.m_ul*(-2*config.l4_y_com*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + 2*config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))) + config.m_ul*(2*config.l4_y_com*sin(theta_3) + 2*config.l4_z_com*cos(theta_3))*(-config.l4_y_com*cos(theta_3) + config.l4_z_com*sin(theta_3))*pow(cos(theta_2), 2) + (-(-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_2) + (-(-config.Iyx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) - config.Izx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_2) - (config.Iyy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_3)*cos(theta_2) - (config.Iyz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_2)*cos(theta_3) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2)*cos(theta_3) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_2))*sin(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + ((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1)*cos(theta_2) + ((config.Iyx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))))*cos(theta_1)*cos(theta_2))*thetadot3 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l4_y*sin(theta_3) + config.l4_z*cos(theta_3) + config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4))) + config.m_ll*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*pow(cos(theta_2), 2) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l4_y*sin(theta_3) + config.l4_z*cos(theta_3) + config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4))*pow(cos(theta_2), 2) + (-(-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_2) + (-(-config.Iyx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) - config.Izx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_2) - (config.Iyy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_3)*cos(theta_2) - (config.Iyz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_2)*cos(theta_3) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2)*cos(theta_3) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_2))*sin(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + ((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1)*cos(theta_2) + ((config.Iyx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))))*cos(theta_1)*cos(theta_2))*thetadot4 + (config.m_foot*(config.l4_y*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_ll*(config.l4_y*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ll*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ul*(config.l4_y_com*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))) + config.m_ul*(-config.l4_y_com*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))) + (-(-config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_2) - (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*sin(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Iyz_ll*cos(theta_2)*cos(theta_3) - config.Izz_ll*sin(theta_3)*cos(theta_2)) + (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Iyy_ll*cos(theta_2)*cos(theta_3) - config.Izy_ll*sin(theta_3)*cos(theta_2)) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (config.Iyx_ll*cos(theta_2)*cos(theta_3) - config.Izx_ll*sin(theta_3)*cos(theta_2))*cos(theta_1)*cos(theta_2))*cos(theta_1)*cos(theta_2) + (-(-config.Iyx_ll*cos(theta_2)*cos(theta_3) + config.Izx_ll*sin(theta_3)*cos(theta_2))*sin(theta_2) - (config.Iyy_ll*cos(theta_2)*cos(theta_3) - config.Izy_ll*sin(theta_3)*cos(theta_2))*sin(theta_3)*cos(theta_2) - (config.Iyz_ll*cos(theta_2)*cos(theta_3) - config.Izz_ll*sin(theta_3)*cos(theta_2))*cos(theta_2)*cos(theta_3) - (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*cos(theta_2)*cos(theta_3) + (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*sin(theta_3)*cos(theta_2))*sin(theta_2) + ((config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + ((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*cos(theta_1)*cos(theta_2) + ((config.Iyx_ll*cos(theta_2)*cos(theta_3) - config.Izx_ll*sin(theta_3)*cos(theta_2))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*cos(theta_2)*cos(theta_3) - config.Izy_ll*sin(theta_3)*cos(theta_2))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*cos(theta_2)*cos(theta_3) - config.Izz_ll*sin(theta_3)*cos(theta_2))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*(-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3)) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*thetadot1 + (config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(-config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(config.l4_y*sin(theta_3) + config.l4_z*cos(theta_3) + config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l4_y*sin(theta_2)*cos(theta_3) - config.l4_z*sin(theta_2)*sin(theta_3) + config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4)*sin(theta_2) + config.l6_y_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) - config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2))*cos(theta_2) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(-config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(config.l4_y*sin(theta_3) + config.l4_z*cos(theta_3) + config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_2) + config.m_ll*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l4_y*sin(theta_2)*cos(theta_3) - config.l4_z*sin(theta_2)*sin(theta_3) + config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4)*sin(theta_2))*cos(theta_2) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(-config.l4_y_com*cos(theta_2)*cos(theta_3) + config.l4_z_com*sin(theta_3)*cos(theta_2))*cos(theta_1) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*cos(theta_1) + config.m_ul*(-config.l4_y_com*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*sin(theta_1) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(-config.l4_y_com*cos(theta_2)*cos(theta_3) + config.l4_z_com*sin(theta_3)*cos(theta_2))*sin(theta_1) + config.m_ul*(config.l4_y_com*sin(theta_3) + config.l4_z_com*cos(theta_3))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3))*cos(theta_2) + config.m_ul*(-config.l4_y_com*cos(theta_3) + config.l4_z_com*sin(theta_3))*(config.l4_y_com*sin(theta_2)*cos(theta_3) - config.l4_z_com*sin(theta_2)*sin(theta_3))*cos(theta_2) + (-(-config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) - config.Izx_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) + (-(-config.Iyx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) - config.Izx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*sin(theta_2) - (config.Iyy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*sin(theta_3)*cos(theta_2) - (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_2)*cos(theta_3) - (config.Iyz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*cos(theta_2)*cos(theta_3) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_3)*cos(theta_2))*sin(theta_2) + ((config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1)*cos(theta_2) + ((config.Iyx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + ((config.Iyx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1)) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) + (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*cos(theta_1)*cos(theta_2))*thetadot2)*thetadot3 - 0.5L*((config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l4_y*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + (-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*thetadot5 + (config.m_foot*(2*config.l4_y*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - 2*config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - 2*config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - 2*config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) - 2*config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - 2*config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(2*config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + 2*config.l4_z*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + 2*config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - 2*config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + 2*config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - 2*config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(2*config.l4_y*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - 2*config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - 2*config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - 2*config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(2*config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + 2*config.l4_z*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + 2*config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - 2*config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ul*(2*config.l4_y_com*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - 2*config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))) + config.m_ul*(2*config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + 2*config.l4_z_com*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))) + (-config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_2) + (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + (-config.Iyx_ll*cos(theta_2)*cos(theta_3) + config.Izx_ll*sin(theta_3)*cos(theta_2))*sin(theta_2) + (config.Iyy_ll*cos(theta_2)*cos(theta_3) - config.Izy_ll*sin(theta_3)*cos(theta_2))*sin(theta_3)*cos(theta_2) + (config.Iyz_ll*cos(theta_2)*cos(theta_3) - config.Izz_ll*sin(theta_3)*cos(theta_2))*cos(theta_2)*cos(theta_3) + (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-config.Ixy_ll*sin(theta_2) + config.Iyy_ll*sin(theta_3)*cos(theta_2) + config.Izy_ll*cos(theta_2)*cos(theta_3))*cos(theta_2)*cos(theta_3) - (-config.Ixz_ll*sin(theta_2) + config.Iyz_ll*sin(theta_3)*cos(theta_2) + config.Izz_ll*cos(theta_2)*cos(theta_3))*sin(theta_3)*cos(theta_2))*thetadot1 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l4_y*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l4_y*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + (-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) + (-config.Iyx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) - config.Izx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_3)*cos(theta_2) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + (config.Iyz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_2)*cos(theta_3) + (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2)*cos(theta_3) - (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_2))*thetadot4 + (config.m_foot*(config.l4_y*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_ll*(config.l4_y*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ll*(-config.l4_y*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ul*(config.l4_y_com*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))) + config.m_ul*(-config.l4_y_com*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))) + (-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) + (-config.Iyx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) - config.Izx_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_3)*cos(theta_2) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + (config.Iyz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*((sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_2)*cos(theta_3) + (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2)*cos(theta_3) - (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_2))*thetadot3 + (config.m_foot*(config.l4_y*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*cos(theta_1) + config.m_foot*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*sin(theta_1) + config.m_ll*(config.l4_y*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)) + config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*cos(theta_1) + config.m_ll*(-config.l4_y*cos(theta_2)*cos(theta_3) + config.l4_z*sin(theta_3)*cos(theta_2) - config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*sin(theta_1) + config.m_ul*(config.l4_y_com*(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*sin(theta_1) + config.m_ul*(config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*cos(theta_1) + config.m_ul*(-config.l4_y_com*cos(theta_2)*cos(theta_3) + config.l4_z_com*sin(theta_3)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)))*cos(theta_1) + config.m_ul*(-config.l4_y_com*cos(theta_2)*cos(theta_3) + config.l4_z_com*sin(theta_3)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)))*sin(theta_1) + (-config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) - config.Izx_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_2) + (-config.Iyx_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) - config.Izx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*sin(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyy_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*sin(theta_3)*cos(theta_2) + (config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*cos(theta_2)*cos(theta_3) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*((-(-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*sin(theta_2)*cos(theta_3) + sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + (config.Iyz_ll*((-sin(theta_1)*sin(theta_3) - sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)) + config.Izz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_1)))*cos(theta_2)*cos(theta_3) - (config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_1)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_1)))*sin(theta_3)*cos(theta_2))*thetadot2)*thetadot1 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l4_z*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) + config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))) + config.m_foot*(config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) - 2*config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*thetadot2*sin(theta_2)*cos(theta_2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l4_y*thetadot3*sin(theta_3) + config.l4_z*thetadot3*cos(theta_3) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4) + config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*thetadot1*sin(theta_1)*cos(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*thetadot2*sin(theta_2)*cos(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*thetadot2*cos(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*cos(theta_2) - (-config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*sin(theta_1)*cos(theta_2) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_2)*cos(theta_1) + (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4)))*sin(theta_1)*cos(theta_2))*thetadot5 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l4_z*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) + config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))) + config.m_foot*(config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) + config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) - 2*config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*thetadot2*sin(theta_2)*cos(theta_2) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l4_y*thetadot3*sin(theta_3) + config.l4_z*thetadot3*cos(theta_3) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4) + config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4) + config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l4_y*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l4_z*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2))) + config.m_ll*(config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4))) - 2*config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*thetadot2*sin(theta_2)*cos(theta_2) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l4_y*thetadot3*sin(theta_3) + config.l4_z*thetadot3*cos(theta_3) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4))*pow(cos(theta_2), 2) + config.m_ll*(config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*pow(cos(theta_2), 2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*thetadot1*sin(theta_1)*cos(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*thetadot2*sin(theta_2)*cos(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*thetadot2*cos(theta_2) + ((config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_2) - (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*cos(theta_2) - (-config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*sin(theta_1)*cos(theta_2) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_2)*cos(theta_1) + (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4)))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot1*sin(theta_1)*cos(theta_2) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_2)*cos(theta_1) + (config.Ixx_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Ixx_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + (-(-config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) - config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*cos(theta_2) - (-config.Ixx_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) - config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_2)*sin(theta_3) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot3*cos(theta_2)*cos(theta_3) - (config.Ixy_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_2)*cos(theta_3) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot3*sin(theta_3)*cos(theta_2) - (config.Ixz_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_2)*cos(theta_3))*sin(theta_2))*thetadot4 + (config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l2_x*thetadot1*cos(theta_1) - config.l2_y*thetadot1*sin(theta_1) + config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - config.l3_y*thetadot1*sin(theta_1) + config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l4_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l4_y*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l4_z*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l5_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l5_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l6_x_com*thetadot1*cos(theta_1)*cos(theta_2) - config.l6_x_com*thetadot2*sin(theta_1)*sin(theta_2) + config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)) - config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*thetadot1*sin(theta_1) + config.l2_y*thetadot1*cos(theta_1) + config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l3_y*thetadot1*cos(theta_1) + config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + config.l4_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l4_y*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l5_x*thetadot2*sin(theta_2)*cos(theta_1) - config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l6_x_com*thetadot1*sin(theta_1)*cos(theta_2) + config.l6_x_com*thetadot2*sin(theta_2)*cos(theta_1) - config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) - config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))) + config.m_foot*(-config.l4_y*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l4_z*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) + config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l4_y*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l2_x*thetadot1*cos(theta_1) - config.l2_y*thetadot1*sin(theta_1) + config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - config.l3_y*thetadot1*sin(theta_1) + config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l4_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l4_y*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l4_z*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l5_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l5_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2))) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*thetadot1*sin(theta_1) + config.l2_y*thetadot1*cos(theta_1) + config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l3_y*thetadot1*cos(theta_1) + config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + config.l4_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l4_y*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l5_x*thetadot2*sin(theta_2)*cos(theta_1) - config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))) + config.m_ll*(-config.l4_y*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l4_z*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l4_y*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l2_x*thetadot1*cos(theta_1) - config.l2_y*thetadot1*sin(theta_1) + config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - config.l3_y*thetadot1*sin(theta_1) + config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + config.l4_x_com*thetadot1*cos(theta_1)*cos(theta_2) - config.l4_x_com*thetadot2*sin(theta_1)*sin(theta_2) + config.l4_y_com*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l4_z_com*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(config.l2_x*thetadot1*sin(theta_1) + config.l2_y*thetadot1*cos(theta_1) + config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l3_y*thetadot1*cos(theta_1) + config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + config.l4_x_com*thetadot1*sin(theta_1)*cos(theta_2) + config.l4_x_com*thetadot2*sin(theta_2)*cos(theta_1) + config.l4_y_com*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))) + config.m_ul*(-config.l4_y_com*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l4_z_com*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x_com*cos(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))) + config.m_ul*(-config.l4_y_com*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x_com*sin(theta_1)*cos(theta_2) + config.l4_y_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))) + (-config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot2*cos(theta_2) + (-config.Ixx_ul*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Izx_ul*(-thetadot2*pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + thetadot2*pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + thetadot2*pow(sin(theta_2), 2) + thetadot2*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(cos(theta_2), 2)))*sin(theta_2) - (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot2*sin(theta_2) + (config.Ixz_ul*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Izz_ul*(-thetadot2*pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + thetadot2*pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + thetadot2*pow(sin(theta_2), 2) + thetadot2*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(cos(theta_2), 2)))*cos(theta_2) + (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*cos(theta_2) + (-config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) + (-config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) - config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*cos(theta_2) + (-config.Ixx_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) - config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_2)*sin(theta_3) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot3*cos(theta_2)*cos(theta_3) + (config.Ixy_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_3)*cos(theta_2) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_2)*cos(theta_3) - (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot3*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_2)*cos(theta_3))*thetadot1 + (config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-2*config.l4_y*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - 2*config.l4_z*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - 2*config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + 2*config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - 2*config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + 2*config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-2*config.l4_y*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + 2*config.l4_z*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + 2*config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + 2*config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) + 2*config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + 2*config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))) - 2*config.m_foot*pow(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5), 2)*thetadot2*sin(theta_2)*cos(theta_2) + config.m_foot*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(2*config.l4_y*thetadot3*sin(theta_3) + 2*config.l4_z*thetadot3*cos(theta_3) + 2*config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + 2*config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4) + 2*config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + 2*config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-2*config.l4_y*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - 2*config.l4_z*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - 2*config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + 2*config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-2*config.l4_y*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + 2*config.l4_z*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + 2*config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + 2*config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2))) - 2*config.m_ll*pow(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4), 2)*thetadot2*sin(theta_2)*cos(theta_2) + config.m_ll*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(2*config.l4_y*thetadot3*sin(theta_3) + 2*config.l4_z*thetadot3*cos(theta_3) + 2*config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + 2*config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4))*pow(cos(theta_2), 2) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(-2*config.l4_y_com*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - 2*config.l4_z_com*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(-2*config.l4_y_com*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + 2*config.l4_z_com*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))) - 2*config.m_ul*pow(-config.l4_y_com*cos(theta_3) + config.l4_z_com*sin(theta_3), 2)*thetadot2*sin(theta_2)*cos(theta_2) + config.m_ul*(-config.l4_y_com*cos(theta_3) + config.l4_z_com*sin(theta_3))*(2*config.l4_y_com*thetadot3*sin(theta_3) + 2*config.l4_z_com*thetadot3*cos(theta_3))*pow(cos(theta_2), 2) + ((config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_2) - (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_2))*thetadot2*cos(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*thetadot1*sin(theta_1)*cos(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*thetadot2*sin(theta_2)*cos(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*thetadot2*cos(theta_2) + ((config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_2) - (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*sin(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - ((config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*sin(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) - ((config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_2)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - ((config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_2)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (-(-config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot2*cos(theta_2) - (-config.Ixx_ul*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Izx_ul*(-thetadot2*pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + thetadot2*pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + thetadot2*pow(sin(theta_2), 2) + thetadot2*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(cos(theta_2), 2)))*sin(theta_2) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot2*sin(theta_2) - (config.Ixz_ul*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Izz_ul*(-thetadot2*pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + thetadot2*pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + thetadot2*pow(sin(theta_2), 2) + thetadot2*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(cos(theta_2), 2)))*cos(theta_2))*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*cos(theta_2) - (-config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*sin(theta_1)*cos(theta_2) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_2)*cos(theta_1) + (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4)))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot1*sin(theta_1)*cos(theta_2) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_2)*cos(theta_1) + (config.Ixx_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Ixx_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + (-(-config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) - config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*cos(theta_2) - (-config.Ixx_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) - config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_2)*sin(theta_3) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot3*cos(theta_2)*cos(theta_3) - (config.Ixy_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_2)*cos(theta_3) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot3*sin(theta_3)*cos(theta_2) - (config.Ixz_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_2)*cos(theta_3))*sin(theta_2) + (-(config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot1*sin(theta_1)*cos(theta_2) - (config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot2*sin(theta_2)*cos(theta_1) + (config.Ixx_ul*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Izx_ul*(-thetadot2*pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + thetadot2*pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + thetadot2*pow(sin(theta_2), 2) + thetadot2*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(cos(theta_2), 2)))*cos(theta_1)*cos(theta_2) + (-config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot1*cos(theta_1) + (-config.Ixy_ul*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Izy_ul*(-thetadot2*pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + thetadot2*pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + thetadot2*pow(sin(theta_2), 2) + thetadot2*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(cos(theta_2), 2)))*sin(theta_1) - (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot1*sin(theta_1)*sin(theta_2) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot2*cos(theta_1)*cos(theta_2) + (config.Ixz_ul*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Izz_ul*(-thetadot2*pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + thetadot2*pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + thetadot2*pow(sin(theta_2), 2) + thetadot2*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(cos(theta_2), 2)))*sin(theta_2)*cos(theta_1))*cos(theta_1)*cos(theta_2) + ((config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Ixx_ul*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Izx_ul*(-thetadot2*pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + thetadot2*pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + thetadot2*pow(sin(theta_2), 2) + thetadot2*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(cos(theta_2), 2)))*sin(theta_1)*cos(theta_2) - (config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot1*sin(theta_1) + (config.Ixy_ul*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Izy_ul*(-thetadot2*pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + thetadot2*pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + thetadot2*pow(sin(theta_2), 2) + thetadot2*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(cos(theta_2), 2)))*cos(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot1*sin(theta_2)*cos(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot2*sin(theta_1)*cos(theta_2) + (config.Ixz_ul*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Izz_ul*(-thetadot2*pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + thetadot2*pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + thetadot2*pow(sin(theta_2), 2) + thetadot2*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(cos(theta_2), 2)))*sin(theta_1)*sin(theta_2))*sin(theta_1)*cos(theta_2))*thetadot3 + (-config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*thetadot1*sin(theta_1) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*thetadot2*cos(theta_2) - config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*thetadot2*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_1) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*thetadot1*cos(theta_1) + config.m_foot*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*thetadot2*cos(theta_2) - config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*thetadot2*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*sin(theta_1) + config.m_foot*(-config.l4_y*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l4_z*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) + config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l4_y*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) - config.m_foot*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*thetadot2*sin(theta_2) + config.m_foot*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l3_x*thetadot2*sin(theta_2) + config.l3_z*thetadot2*cos(theta_2) - config.l4_x*thetadot2*sin(theta_2) + config.l4_y*thetadot2*sin(theta_3)*cos(theta_2) + config.l4_y*thetadot3*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot2*cos(theta_2)*cos(theta_3) - config.l4_z*thetadot3*sin(theta_2)*sin(theta_3) - config.l5_x*thetadot2*sin(theta_2) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*thetadot2*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x_com*thetadot2*sin(theta_2) + config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_2) + config.m_foot*(config.l4_y*thetadot3*sin(theta_3) + config.l4_z*thetadot3*cos(theta_3) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4) + config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) - config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*thetadot1*sin(theta_1) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_1) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*thetadot1*cos(theta_1) + config.m_ll*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4))*sin(theta_1) + config.m_ll*(-config.l4_y*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l4_z*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l4_y*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) - config.m_ll*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*thetadot2*sin(theta_2) + config.m_ll*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(-config.l3_x*thetadot2*sin(theta_2) + config.l3_z*thetadot2*cos(theta_2) - config.l4_x*thetadot2*sin(theta_2) + config.l4_y*thetadot2*sin(theta_3)*cos(theta_2) + config.l4_y*thetadot3*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot2*cos(theta_2)*cos(theta_3) - config.l4_z*thetadot3*sin(theta_2)*sin(theta_3) - config.l5_x*thetadot2*sin(theta_2) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*thetadot2*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_2) + config.m_ll*(config.l4_y*thetadot3*sin(theta_3) + config.l4_z*thetadot3*cos(theta_3) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_2) - config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*thetadot1*sin(theta_1) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z_com*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x_com*thetadot2*cos(theta_2) + config.l4_y_com*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y_com*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z_com*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z_com*thetadot3*sin(theta_3)*cos(theta_2))*cos(theta_1) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*thetadot1*cos(theta_1) + config.m_ul*(-config.l4_y_com*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z_com*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x_com*thetadot2*cos(theta_2) + config.l4_y_com*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y_com*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z_com*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z_com*thetadot3*sin(theta_3)*cos(theta_2))*sin(theta_1) + config.m_ul*(-config.l4_y_com*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l4_z_com*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*sin(theta_1) + config.m_ul*(-config.l4_y_com*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z_com*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x_com*sin(theta_2) - config.l4_y_com*sin(theta_3)*cos(theta_2) - config.l4_z_com*cos(theta_2)*cos(theta_3))*cos(theta_1) - config.m_ul*(-config.l4_y_com*cos(theta_3) + config.l4_z_com*sin(theta_3))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3))*thetadot2*sin(theta_2) + config.m_ul*(-config.l4_y_com*cos(theta_3) + config.l4_z_com*sin(theta_3))*(-config.l3_x*thetadot2*sin(theta_2) + config.l3_z*thetadot2*cos(theta_2) - config.l4_x_com*thetadot2*sin(theta_2) + config.l4_y_com*thetadot2*sin(theta_3)*cos(theta_2) + config.l4_y_com*thetadot3*sin(theta_2)*cos(theta_3) + config.l4_z_com*thetadot2*cos(theta_2)*cos(theta_3) - config.l4_z_com*thetadot3*sin(theta_2)*sin(theta_3))*cos(theta_2) + config.m_ul*(config.l4_y_com*thetadot3*sin(theta_3) + config.l4_z_com*thetadot3*cos(theta_3))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3))*cos(theta_2) + (-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*thetadot1*cos(theta_1) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*thetadot1*sin(theta_1) - ((config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1)*sin(theta_2))*thetadot1*sin(theta_1) + (-(config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_1) - (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*sin(theta_2)*cos(theta_1))*thetadot1*cos(theta_1) + (-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*sin(theta_1)*cos(theta_2) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_2)*cos(theta_1) - (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) - (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot1*sin(theta_1)*cos(theta_2) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_2)*cos(theta_1) - (config.Ixx_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Ixx_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)))*cos(theta_1) + ((config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot1*sin(theta_1)*cos(theta_2) + (config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot2*sin(theta_2)*cos(theta_1) - (config.Ixx_ul*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Izx_ul*(-thetadot2*pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + thetadot2*pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + thetadot2*pow(sin(theta_2), 2) + thetadot2*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(cos(theta_2), 2)))*cos(theta_1)*cos(theta_2) - (-config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot1*cos(theta_1) - (-config.Ixy_ul*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Izy_ul*(-thetadot2*pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + thetadot2*pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + thetadot2*pow(sin(theta_2), 2) + thetadot2*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(cos(theta_2), 2)))*sin(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot1*sin(theta_1)*sin(theta_2) - (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot2*cos(theta_1)*cos(theta_2) - (config.Ixz_ul*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Izz_ul*(-thetadot2*pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + thetadot2*pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + thetadot2*pow(sin(theta_2), 2) + thetadot2*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(cos(theta_2), 2)))*sin(theta_2)*cos(theta_1))*sin(theta_1) + ((config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Ixx_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izx_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Ixx_ul*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Izx_ul*(-thetadot2*pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + thetadot2*pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + thetadot2*pow(sin(theta_2), 2) + thetadot2*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(cos(theta_2), 2)))*sin(theta_1)*cos(theta_2) - (config.Ixy_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izy_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot1*sin(theta_1) + (config.Ixy_ul*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Izy_ul*(-thetadot2*pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + thetadot2*pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + thetadot2*pow(sin(theta_2), 2) + thetadot2*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(cos(theta_2), 2)))*cos(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot1*sin(theta_2)*cos(theta_1) + (config.Ixz_ul*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Izz_ul*(pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) + sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) - sin(theta_2)*cos(theta_2)))*thetadot2*sin(theta_1)*cos(theta_2) + (config.Ixz_ul*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Izz_ul*(-thetadot2*pow(sin(theta_1), 2)*pow(sin(theta_2), 2) + thetadot2*pow(sin(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(sin(theta_2), 2)*pow(cos(theta_1), 2) + thetadot2*pow(sin(theta_2), 2) + thetadot2*pow(cos(theta_1), 2)*pow(cos(theta_2), 2) - thetadot2*pow(cos(theta_2), 2)))*sin(theta_1)*sin(theta_2))*cos(theta_1))*thetadot2;

    double temp_4 = -0.5L*((config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-2*config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + 2*config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(2*config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + 2*config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(2*config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + 2*config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + (-(-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1)*cos(theta_2))*thetadot5 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + (-(-config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_2) - (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*sin(theta_2) + ((config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*cos(theta_1)*cos(theta_2))*thetadot1 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + (-(-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1)*cos(theta_2))*thetadot4 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + (-(-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1)*cos(theta_2))*thetadot3 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4)*sin(theta_2) + config.l6_y_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) - config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2))*cos(theta_2) + (-(-config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) + ((config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1)*cos(theta_2))*thetadot2)*thetadot5 - 0.5L*((config.m_foot*(2*config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - 2*config.l5_z*sin(theta_3 + theta_4)*sin(theta_2) + 2*config.l6_y_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) - 2*config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.m_foot*(-2*config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - 2*config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + 2*config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*pow(sin(theta_1), 2) + config.m_foot*(-2*config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - 2*config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + 2*config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*pow(cos(theta_1), 2) + config.m_ll*(2*config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - 2*config.l5_z*sin(theta_3 + theta_4)*sin(theta_2))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4)) + config.m_ll*(-2*config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*pow(sin(theta_1), 2) + config.m_ll*(-2*config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + 2*config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*pow(cos(theta_1), 2) + ((config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*cos(theta_1) + (-(config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) - (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*sin(theta_1))*thetadot2 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4)*sin(theta_2) + config.l6_y_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) - config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2))*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1) + (-(config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) - (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1))*thetadot5 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*cos(theta_1) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*sin(theta_1) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*cos(theta_1) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*sin(theta_1) + ((config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*cos(theta_1) + (-(config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2) - (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) - (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*sin(theta_1))*thetadot1 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4)*sin(theta_2) + config.l6_y_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) - config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2))*cos(theta_2) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_2) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4)*sin(theta_2))*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1) + (-(config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) - (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1))*thetadot4 + (config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4)*sin(theta_2) + config.l6_y_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) - config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*cos(theta_1) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*sin(theta_1) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_2) + config.m_ll*(config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4)*sin(theta_2))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*cos(theta_2) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*cos(theta_1) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*sin(theta_1) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1) + (-(config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) - (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1))*thetadot3)*thetadot2 - 0.5L*((config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + (-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*thetadot5 + (config.m_foot*(-2*config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - 2*config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) - 2*config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - 2*config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(2*config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - 2*config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + 2*config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - 2*config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(-2*config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - 2*config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(2*config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - 2*config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + (-config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_2) + (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*thetadot1 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + (-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*thetadot4 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + (-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*thetadot3 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*cos(theta_1) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*sin(theta_1) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*cos(theta_1) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*sin(theta_1) + (-config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*thetadot2)*thetadot1 - 0.5L*((config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + (-(-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1)*cos(theta_2))*thetadot5 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-2*config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + 2*config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - 2*config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + 2*config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(2*config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + 2*config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + 2*config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + 2*config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(2*config.l5_y*sin(theta_3 + theta_4) + 2*config.l5_z*cos(theta_3 + theta_4) + 2*config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + 2*config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-2*config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + 2*config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(2*config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + 2*config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ll*(2*config.l5_y*sin(theta_3 + theta_4) + 2*config.l5_z*cos(theta_3 + theta_4))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*pow(cos(theta_2), 2) + (-(-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1)*cos(theta_2))*thetadot4 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + (-(-config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_2) - (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*sin(theta_2) + ((config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*cos(theta_1)*cos(theta_2))*thetadot1 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4))) + config.m_ll*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*pow(cos(theta_2), 2) + config.m_ll*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*pow(cos(theta_2), 2) + (-(-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1)*cos(theta_2))*thetadot3 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4)*sin(theta_2) + config.l6_y_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) - config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2))*cos(theta_2) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_2) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4)*sin(theta_2))*cos(theta_2) + (-(-config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) + ((config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1)*cos(theta_2))*thetadot2)*thetadot4 - 0.5L*((config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + (-(-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1)*cos(theta_2))*thetadot5 + (config.m_foot*(2*config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + 2*config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + 2*config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + 2*config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-2*config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + 2*config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - 2*config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + 2*config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(2*config.l5_y*sin(theta_3 + theta_4) + 2*config.l5_z*cos(theta_3 + theta_4) + 2*config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + 2*config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_ll*(2*config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + 2*config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-2*config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + 2*config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4))) + config.m_ll*(2*config.l5_y*sin(theta_3 + theta_4) + 2*config.l5_z*cos(theta_3 + theta_4))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*pow(cos(theta_2), 2) + (-(-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1)*cos(theta_2))*thetadot3 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(config.l5_y*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + (-(-config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_2) - (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*sin(theta_2) + ((config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izx_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izy_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) + config.Izz_foot*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(-config.Ixz_foot*sin(theta_2) + config.Iyz_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izz_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(-config.Ixy_foot*sin(theta_2) + config.Iyy_foot*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) + config.Izy_foot*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))))*cos(theta_1)*cos(theta_2))*thetadot1 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4))) + config.m_ll*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*pow(cos(theta_2), 2) + config.m_ll*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*pow(cos(theta_2), 2) + (-(-config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))))*cos(theta_1)*cos(theta_2))*thetadot4 + (config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4)*sin(theta_2) + config.l6_y_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) - config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*cos(theta_1) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*sin(theta_1) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(-sin(theta_3 + theta_4)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l5_y*(-sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) + config.m_ll*(config.l5_y*sin(theta_3 + theta_4) + config.l5_z*cos(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_2) + config.m_ll*(config.l5_y*sin(theta_2)*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4)*sin(theta_2))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*cos(theta_2) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*cos(theta_1) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_z*sin(theta_3 + theta_4)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*sin(theta_1) + (-(-config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_2) - (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4)))*sin(theta_2) + ((config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*sin(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1)*cos(theta_2) + ((config.Iyx_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*cos(theta_1)*cos(theta_2) + (config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Iyy_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Iyz_foot*((-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) - (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1)*cos(theta_2))*thetadot2)*thetadot3 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) + config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))) + config.m_foot*(config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) - 2*config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*thetadot2*sin(theta_2)*cos(theta_2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4) + config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*thetadot1*sin(theta_1)*cos(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*thetadot2*sin(theta_2)*cos(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*thetadot2*cos(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*cos(theta_2) - (-config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*sin(theta_1)*cos(theta_2) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_2)*cos(theta_1) + (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4)))*sin(theta_1)*cos(theta_2))*thetadot5 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l2_x*thetadot1*cos(theta_1) - config.l2_y*thetadot1*sin(theta_1) + config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - config.l3_y*thetadot1*sin(theta_1) + config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l4_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l4_y*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l4_z*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l5_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l5_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l6_x_com*thetadot1*cos(theta_1)*cos(theta_2) - config.l6_x_com*thetadot2*sin(theta_1)*sin(theta_2) + config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)) - config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*thetadot1*sin(theta_1) + config.l2_y*thetadot1*cos(theta_1) + config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l3_y*thetadot1*cos(theta_1) + config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + config.l4_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l4_y*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l5_x*thetadot2*sin(theta_2)*cos(theta_1) - config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l6_x_com*thetadot1*sin(theta_1)*cos(theta_2) + config.l6_x_com*thetadot2*sin(theta_2)*cos(theta_1) - config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) - config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))) + config.m_foot*(config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) + config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l2_x*thetadot1*cos(theta_1) - config.l2_y*thetadot1*sin(theta_1) + config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - config.l3_y*thetadot1*sin(theta_1) + config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l4_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l4_y*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l4_z*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l5_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l5_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l2_x*thetadot1*sin(theta_1) + config.l2_y*thetadot1*cos(theta_1) + config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l3_y*thetadot1*cos(theta_1) + config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + config.l4_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l4_y*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l5_x*thetadot2*sin(theta_2)*cos(theta_1) - config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))) + config.m_ll*(config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4))) + (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*cos(theta_2) + (-config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) + (-config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) - config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*cos(theta_2) + (-config.Ixx_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) - config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_2)*sin(theta_3) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot3*cos(theta_2)*cos(theta_3) + (config.Ixy_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_3)*cos(theta_2) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)) - (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_2)*cos(theta_3) - (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot3*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_2)*cos(theta_3))*thetadot1 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-2*config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + 2*config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - 2*config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + 2*config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(2*config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + 2*config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) + 2*config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + 2*config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))) - 2*config.m_foot*pow(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5), 2)*thetadot2*sin(theta_2)*cos(theta_2) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(2*config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + 2*config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4) + 2*config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + 2*config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-2*config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + 2*config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(2*config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + 2*config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2))) - 2*config.m_ll*pow(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4), 2)*thetadot2*sin(theta_2)*cos(theta_2) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(2*config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + 2*config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4))*pow(cos(theta_2), 2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*thetadot1*sin(theta_1)*cos(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*thetadot2*sin(theta_2)*cos(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*thetadot2*cos(theta_2) + ((config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_2) - (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*cos(theta_2) - (-config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*sin(theta_1)*cos(theta_2) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_2)*cos(theta_1) + (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4)))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot1*sin(theta_1)*cos(theta_2) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_2)*cos(theta_1) + (config.Ixx_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Ixx_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + (-(-config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) - config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*cos(theta_2) - (-config.Ixx_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) - config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_2)*sin(theta_3) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot3*cos(theta_2)*cos(theta_3) - (config.Ixy_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_2)*cos(theta_3) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot3*sin(theta_3)*cos(theta_2) - (config.Ixz_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_2)*cos(theta_3))*sin(theta_2))*thetadot4 + (-config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*thetadot1*sin(theta_1) + config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*thetadot2*cos(theta_2) - config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*thetadot2*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_1) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*thetadot1*cos(theta_1) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*thetadot2*cos(theta_2) - config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*thetadot2*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*sin(theta_1) + config.m_foot*(config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) + config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) - config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*thetadot2*sin(theta_2) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l3_x*thetadot2*sin(theta_2) + config.l3_z*thetadot2*cos(theta_2) - config.l4_x*thetadot2*sin(theta_2) + config.l4_y*thetadot2*sin(theta_3)*cos(theta_2) + config.l4_y*thetadot3*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot2*cos(theta_2)*cos(theta_3) - config.l4_z*thetadot3*sin(theta_2)*sin(theta_3) - config.l5_x*thetadot2*sin(theta_2) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*thetadot2*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x_com*thetadot2*sin(theta_2) + config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_2) + config.m_foot*(config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4) + config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) - config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*thetadot1*sin(theta_1) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_1) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*thetadot1*cos(theta_1) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4))*sin(theta_1) + config.m_ll*(config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*sin(theta_1) + config.m_ll*(-config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_1) - config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*thetadot2*sin(theta_2) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(-config.l3_x*thetadot2*sin(theta_2) + config.l3_z*thetadot2*cos(theta_2) - config.l4_x*thetadot2*sin(theta_2) + config.l4_y*thetadot2*sin(theta_3)*cos(theta_2) + config.l4_y*thetadot3*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot2*cos(theta_2)*cos(theta_3) - config.l4_z*thetadot3*sin(theta_2)*sin(theta_3) - config.l5_x*thetadot2*sin(theta_2) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*thetadot2*cos(theta_3 + theta_4)*cos(theta_2))*cos(theta_2) + config.m_ll*(config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4))*cos(theta_2) + (-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*thetadot1*cos(theta_1) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*thetadot1*sin(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*thetadot1*cos(theta_1) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*thetadot1*sin(theta_1) + (-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*sin(theta_1)*cos(theta_2) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_2)*cos(theta_1) - (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) - (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot1*sin(theta_1)*cos(theta_2) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_2)*cos(theta_1) - (config.Ixx_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)))*sin(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Ixx_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)))*cos(theta_1))*thetadot2 + (config.m_foot*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))) + config.m_foot*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l4_z*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) + config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))) + config.m_foot*(config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) + config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) - 2*config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*thetadot2*sin(theta_2)*cos(theta_2) + config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l4_y*thetadot3*sin(theta_3) + config.l4_z*thetadot3*cos(theta_3) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4) + config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4) + config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_ll*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)))*(-config.l4_y*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2))) + config.m_ll*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)))*(-config.l4_y*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l4_z*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2))) + config.m_ll*(config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1))) + config.m_ll*(-config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4))) - 2*config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*thetadot2*sin(theta_2)*cos(theta_2) + config.m_ll*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*(config.l4_y*thetadot3*sin(theta_3) + config.l4_z*thetadot3*cos(theta_3) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4))*pow(cos(theta_2), 2) + config.m_ll*(config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4))*pow(cos(theta_2), 2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*thetadot1*sin(theta_1)*cos(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*thetadot2*sin(theta_2)*cos(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_1)*cos(theta_2))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_1)*cos(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*thetadot2*cos(theta_2) + ((config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_2) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*sin(theta_3)*cos(theta_2) - (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*cos(theta_2) - (-config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*sin(theta_1)*cos(theta_2) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_2)*cos(theta_1) + (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4)))*sin(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*(config.Ixz_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*(config.Ixy_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot1*sin(theta_1)*cos(theta_2) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_2)*cos(theta_1) + (config.Ixx_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*(config.Ixy_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*(config.Ixz_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2))) + (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Ixx_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)))*sin(theta_1)*cos(theta_2) + (-(-config.Ixx_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) - config.Izx_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*cos(theta_2) - (-config.Ixx_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Iyx_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) - config.Izx_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_2) + (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_2)*sin(theta_3) - (config.Ixy_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izy_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot3*cos(theta_2)*cos(theta_3) - (config.Ixy_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izy_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*sin(theta_3)*cos(theta_2) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot2*sin(theta_2)*cos(theta_3) + (config.Ixz_ll*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_ll*((-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) - sin(theta_2)*sin(theta_3)*cos(theta_2)) + config.Izz_ll*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) - sin(theta_2)*cos(theta_2)*cos(theta_3)))*thetadot3*sin(theta_3)*cos(theta_2) - (config.Ixz_ll*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_ll*(-(-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot1*sin(theta_1)*cos(theta_2) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_1)*sin(theta_2) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*sin(theta_3) - thetadot2*sin(theta_3)*pow(cos(theta_2), 2) - thetadot3*sin(theta_2)*cos(theta_2)*cos(theta_3)) + config.Izz_ll*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot1*sin(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot2*sin(theta_2)*cos(theta_1) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot1*cos(theta_1)*cos(theta_2) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot2*sin(theta_1)*sin(theta_2) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_1)*cos(theta_2) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_1)*cos(theta_2) + thetadot2*pow(sin(theta_2), 2)*cos(theta_3) - thetadot2*pow(cos(theta_2), 2)*cos(theta_3) + thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_2)))*cos(theta_2)*cos(theta_3))*sin(theta_2))*thetadot3;

    double temp_5 = -0.5L*((config.m_foot*(-2*config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - 2*config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(2*config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - 2*config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))))*thetadot1 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))))*thetadot5 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))))*thetadot4 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))))*thetadot3 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*cos(theta_1) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*sin(theta_1))*thetadot2)*thetadot1 - 0.5L*((config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-2*config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + 2*config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(2*config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + 2*config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(2*config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + 2*config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2))*thetadot5 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))))*thetadot1 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2))*thetadot4 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2))*thetadot3 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l6_y_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) - config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2))*cos(theta_2))*thetadot2)*thetadot5 - 0.5L*((config.m_foot*(2*config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + 2*config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-2*config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + 2*config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(2*config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + 2*config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2))*thetadot4 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))))*thetadot1 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2))*thetadot5 + (config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2))*thetadot3 + (config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(config.l6_y_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) - config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*cos(theta_1) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*sin(theta_1))*thetadot2)*thetadot4 - 0.5L*((config.m_foot*(2*config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + 2*config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-2*config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + 2*config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(2*config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + 2*config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2))*thetadot3 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))))*thetadot1 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2))*thetadot5 + (config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2))*thetadot4 + (config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(config.l6_y_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) - config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*cos(theta_1) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*sin(theta_1))*thetadot2)*thetadot3 - 0.5L*((config.m_foot*(2*config.l6_y_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) - 2*config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.m_foot*(-2*config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + 2*config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*pow(sin(theta_1), 2) + config.m_foot*(-2*config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + 2*config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*pow(cos(theta_1), 2))*thetadot2 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*cos(theta_1) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*sin(theta_1))*thetadot1 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l6_y_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) - config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2))*cos(theta_2))*thetadot5 + (config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(config.l6_y_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) - config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*cos(theta_1) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*sin(theta_1))*thetadot4 + (config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(-sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l6_y_com*(-sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) + config.m_foot*(config.l6_y_com*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(config.l6_y_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) - config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*cos(theta_1) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*sin(theta_1))*thetadot3)*thetadot2 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l2_x*thetadot1*cos(theta_1) - config.l2_y*thetadot1*sin(theta_1) + config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - config.l3_y*thetadot1*sin(theta_1) + config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l4_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l4_y*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l4_z*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l5_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l5_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l6_x_com*thetadot1*cos(theta_1)*cos(theta_2) - config.l6_x_com*thetadot2*sin(theta_1)*sin(theta_2) + config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)) - config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l2_x*thetadot1*sin(theta_1) + config.l2_y*thetadot1*cos(theta_1) + config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l3_y*thetadot1*cos(theta_1) + config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + config.l4_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l4_y*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l5_x*thetadot2*sin(theta_2)*cos(theta_1) - config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l6_x_com*thetadot1*sin(theta_1)*cos(theta_2) + config.l6_x_com*thetadot2*sin(theta_2)*cos(theta_1) - config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) - config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))) + config.m_foot*(config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)))*(-config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x_com*cos(theta_1)*cos(theta_2) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)))*(config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x_com*sin(theta_1)*cos(theta_2) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))) + (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*cos(theta_2) + (-config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*thetadot1 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-2*config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + 2*config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(2*config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + 2*config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))) - 2*config.m_foot*pow(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5), 2)*thetadot2*sin(theta_2)*cos(theta_2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(2*config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + 2*config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*thetadot1*sin(theta_1)*cos(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*thetadot2*sin(theta_2)*cos(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*thetadot2*cos(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*cos(theta_2) - (-config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*sin(theta_1)*cos(theta_2) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_2)*cos(theta_1) + (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4)))*sin(theta_1)*cos(theta_2))*thetadot5 + (-config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*thetadot1*sin(theta_1) + config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*thetadot2*cos(theta_2) - config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*thetadot2*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*thetadot1*cos(theta_1) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*thetadot2*cos(theta_2) - config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z_com*thetadot2*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*sin(theta_1) + config.m_foot*(config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1) + config.m_foot*(-config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)))*(config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x_com*sin(theta_2) - config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1) - config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*thetadot2*sin(theta_2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l3_x*thetadot2*sin(theta_2) + config.l3_z*thetadot2*cos(theta_2) - config.l4_x*thetadot2*sin(theta_2) + config.l4_y*thetadot2*sin(theta_3)*cos(theta_2) + config.l4_y*thetadot3*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot2*cos(theta_2)*cos(theta_3) - config.l4_z*thetadot3*sin(theta_2)*sin(theta_3) - config.l5_x*thetadot2*sin(theta_2) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*thetadot2*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x_com*thetadot2*sin(theta_2) + config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + config.l6_y_com*thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_2) + config.m_foot*(config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2) + (-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*thetadot1*cos(theta_1) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*thetadot1*sin(theta_1) + (-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*sin(theta_1)*cos(theta_2) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_2)*cos(theta_1) - (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) - (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) - (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)))*sin(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4)))*cos(theta_1))*thetadot2 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) + config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))) + config.m_foot*(config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)))*(config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)))*(-config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) - 2*config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*thetadot2*sin(theta_2)*cos(theta_2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4) + config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*thetadot1*sin(theta_1)*cos(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*thetadot2*sin(theta_2)*cos(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*thetadot2*cos(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*cos(theta_2) - (-config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*sin(theta_1)*cos(theta_2) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_2)*cos(theta_1) + (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4)))*sin(theta_1)*cos(theta_2))*thetadot4 + (config.m_foot*(-config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)))*(-config.l4_y*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2))) + config.m_foot*(config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)))*(-config.l4_y*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l4_z*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) + config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2))) + config.m_foot*(config.l6_y_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) - thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)))*(-config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1))) + config.m_foot*(-config.l6_y_com*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z_com*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)))*(-config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z_com*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5))) - 2*config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*thetadot2*sin(theta_2)*cos(theta_2) + config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*(config.l4_y*thetadot3*sin(theta_3) + config.l4_z*thetadot3*cos(theta_3) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4) + config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) + config.m_foot*(config.l6_y_com*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + config.l6_z_com*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*pow(cos(theta_2), 2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*thetadot1*sin(theta_1)*cos(theta_2) - (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*cos(theta_1)*cos(theta_2))*thetadot2*sin(theta_2)*cos(theta_1) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*thetadot1*cos(theta_1)*cos(theta_2) - ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_1)*cos(theta_2))*thetadot2*sin(theta_1)*sin(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*sin(theta_2))*thetadot2*cos(theta_2) + (-(-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (-config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) - config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) - config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*cos(theta_2) - (-config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) - config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) - config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_2) - (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) - thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) - thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) - thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) + thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4)) - (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) - thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) - thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) - thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) - thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3)))*sin(theta_2) + (((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*sin(theta_1)*cos(theta_2) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_2)*cos(theta_1) + (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*cos(theta_1)*cos(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4)))*cos(theta_1)*cos(theta_2) + ((-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*(config.Ixz_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izz_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*(config.Ixy_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izy_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2))) + (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot1*cos(theta_1)*cos(theta_2) - (config.Ixx_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izx_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*thetadot2*sin(theta_1)*sin(theta_2) + (config.Ixx_foot*(-2*thetadot2*pow(sin(theta_1), 2)*sin(theta_2)*cos(theta_2) - 2*thetadot2*sin(theta_2)*pow(cos(theta_1), 2)*cos(theta_2) + 2*thetadot2*sin(theta_2)*cos(theta_2)) + config.Iyx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (-sin(theta_3)*cos(theta_2)*cos(theta_4) - sin(theta_4)*cos(theta_2)*cos(theta_3))*thetadot2*cos(theta_2) + ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) + (thetadot2*sin(theta_2)*sin(theta_3)*cos(theta_4) + thetadot2*sin(theta_2)*sin(theta_4)*cos(theta_3) + thetadot3*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot3*cos(theta_2)*cos(theta_3)*cos(theta_4) + thetadot4*sin(theta_3)*sin(theta_4)*cos(theta_2) - thetadot4*cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)) + config.Izx_foot*(-((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot1*sin(theta_1)*cos(theta_2) - ((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*thetadot2*sin(theta_2)*cos(theta_1) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot1*cos(theta_1)*cos(theta_2) - (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*thetadot2*sin(theta_1)*sin(theta_2) + (sin(theta_3)*sin(theta_4)*cos(theta_2) - cos(theta_2)*cos(theta_3)*cos(theta_4))*thetadot2*cos(theta_2) + (-(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) + thetadot1*cos(theta_1)*cos(theta_3) - thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_3) - thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) + (-thetadot2*sin(theta_2)*sin(theta_3)*sin(theta_4) + thetadot2*sin(theta_2)*cos(theta_3)*cos(theta_4) + thetadot3*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot3*sin(theta_4)*cos(theta_2)*cos(theta_3) + thetadot4*sin(theta_3)*cos(theta_2)*cos(theta_4) + thetadot4*sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)))*sin(theta_1)*cos(theta_2) + (config.Ixy_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izy_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*(-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*thetadot4*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*cos(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1))*cos(theta_4)) + (config.Ixz_foot*(pow(sin(theta_1), 2)*pow(cos(theta_2), 2) + pow(sin(theta_2), 2) + pow(cos(theta_1), 2)*pow(cos(theta_2), 2)) + config.Iyz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*sin(theta_4) + (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*cos(theta_4))*cos(theta_1)*cos(theta_2) + ((sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*cos(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*sin(theta_4))*sin(theta_1)*cos(theta_2) - (sin(theta_3)*cos(theta_2)*cos(theta_4) + sin(theta_4)*cos(theta_2)*cos(theta_3))*sin(theta_2)) + config.Izz_foot*(((sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3))*cos(theta_4) - (-sin(theta_1)*cos(theta_3) + sin(theta_2)*sin(theta_3)*cos(theta_1))*sin(theta_4))*cos(theta_1)*cos(theta_2) + (-(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3))*sin(theta_4) + (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*cos(theta_4))*sin(theta_1)*cos(theta_2) - (-sin(theta_3)*sin(theta_4)*cos(theta_2) + cos(theta_2)*cos(theta_3)*cos(theta_4))*sin(theta_2)))*((-sin(theta_1)*sin(theta_2)*sin(theta_3) - cos(theta_1)*cos(theta_3))*thetadot4*cos(theta_4) - (sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1))*thetadot4*sin(theta_4) + (thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3))*cos(theta_4) + (thetadot1*sin(theta_1)*cos(theta_3) - thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) - thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) - thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot3*sin(theta_3)*cos(theta_1))*sin(theta_4)))*sin(theta_1)*cos(theta_2))*thetadot3;


    //This is the vector already multiplied by q_dot
    C << temp_1, temp_2, temp_3, temp_4, temp_5;
}

void update_G(double theta_1, double theta_2, double theta_3, double theta_4, double theta_5, Eigen::Matrix<double, 5, 1> &G, leg_config &config) {
    
    double temp_1 = 0;

    double temp_2 = config.g*config.m_foot*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x_com*cos(theta_2) + config.l6_y_com*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z_com*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.g*config.m_hip1*(config.l3_x_com*cos(theta_2) + config.l3_z_com*sin(theta_2)) + config.g*config.m_ll*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x_com*cos(theta_2) + config.l5_y_com*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z_com*sin(theta_2)*cos(theta_3 + theta_4)) + config.g*config.m_ul*(config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x_com*cos(theta_2) + config.l4_y_com*sin(theta_2)*sin(theta_3) + config.l4_z_com*sin(theta_2)*cos(theta_3));

    double temp_3 = config.g*config.m_foot*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.g*config.m_ll*(-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y_com*cos(theta_3 + theta_4) + config.l5_z_com*sin(theta_3 + theta_4))*cos(theta_2) + config.g*config.m_ul*(-config.l4_y_com*cos(theta_3) + config.l4_z_com*sin(theta_3))*cos(theta_2);

    double temp_4 = config.g*config.m_foot*(-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*cos(theta_2) + config.g*config.m_ll*(-config.l5_y_com*cos(theta_3 + theta_4) + config.l5_z_com*sin(theta_3 + theta_4))*cos(theta_2);

    double temp_5 = config.g*config.m_foot*(-config.l6_y_com*cos(theta_3 + theta_4 + theta_5) + config.l6_z_com*sin(theta_3 + theta_4 + theta_5))*cos(theta_2);


    G << temp_1, temp_2, temp_3, temp_4, temp_5;
}


void update_J_foot(double theta_1, double theta_2, double theta_3, double theta_4, double theta_5, Eigen::Matrix<double, 3, 5> &J_foot, leg_config &config) {

    double temp_1 = config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x*sin(theta_1)*cos(theta_2) + config.l6_y*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5));

    double temp_2 = (config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x*sin(theta_2) - config.l6_y*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1);

    double temp_3 = -config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5));

    double temp_4 = -config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5));

    double temp_5 = -config.l6_y*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5));

    double temp_6 = -config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x*cos(theta_1)*cos(theta_2) - config.l6_y*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1));

    double temp_7 = (config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x*sin(theta_2) - config.l6_y*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1);

    double temp_8 = -config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1));

    double temp_9 = config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1));

    double temp_10 = config.l6_y*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1));

    double temp_11 = 0;

    double temp_12 = config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x*cos(theta_2) + config.l6_y*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z*sin(theta_2)*cos(theta_3 + theta_4 + theta_5);

    double temp_13 = (-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y*cos(theta_3 + theta_4 + theta_5) + config.l6_z*sin(theta_3 + theta_4 + theta_5))*cos(theta_2);

    double temp_14 = (-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y*cos(theta_3 + theta_4 + theta_5) + config.l6_z*sin(theta_3 + theta_4 + theta_5))*cos(theta_2);

    double temp_15 = (-config.l6_y*cos(theta_3 + theta_4 + theta_5) + config.l6_z*sin(theta_3 + theta_4 + theta_5))*cos(theta_2);

    J_foot << temp_1, temp_2, temp_3, temp_4, temp_5,
                        temp_6, temp_7, temp_8, temp_9, temp_10,
                        temp_11, temp_12, temp_13, temp_14, temp_15;
}

void update_J_foot_combined(double theta_1, double theta_2, double theta_3, double theta_4, double theta_5, double thetadot1, double thetadot2, double thetadot3, double thetadot4, double thetadot5, Eigen::Matrix<double, 5, 5> &J_foot_combined, leg_config &config) {

    double temp_1 = config.l2_x*sin(theta_1) + config.l2_y*cos(theta_1) + config.l3_x*sin(theta_1)*cos(theta_2) + config.l3_y*cos(theta_1) + config.l3_z*sin(theta_1)*sin(theta_2) + config.l4_x*sin(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l5_x*sin(theta_1)*cos(theta_2) + config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) - config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l6_x*sin(theta_1)*cos(theta_2) + config.l6_y*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) - config.l6_z*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5));

    double temp_2 = (config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x*sin(theta_2) - config.l6_y*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*cos(theta_1);

    double temp_3 = -config.l4_y*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5));

    double temp_4 = -config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l6_y*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5));

    double temp_5 = -config.l6_y*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5));

    double temp_6 = -config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x*cos(theta_1)*cos(theta_2) - config.l6_y*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1));

    double temp_7 = (config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x*sin(theta_2) - config.l6_y*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*sin(theta_1);

    double temp_8 = -config.l4_y*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) + config.l4_z*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) + config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1));

    double temp_9 = config.l5_y*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) + config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l6_y*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1));

    double temp_10 = config.l6_y*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)) + config.l6_z*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1));

    double temp_11 = 0;

    double temp_12 = config.l3_x*cos(theta_2) + config.l3_z*sin(theta_2) + config.l4_x*cos(theta_2) + config.l4_y*sin(theta_2)*sin(theta_3) + config.l4_z*sin(theta_2)*cos(theta_3) + config.l5_x*cos(theta_2) + config.l5_y*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x*cos(theta_2) + config.l6_y*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z*sin(theta_2)*cos(theta_3 + theta_4 + theta_5);

    double temp_13 = (-config.l4_y*cos(theta_3) + config.l4_z*sin(theta_3) - config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y*cos(theta_3 + theta_4 + theta_5) + config.l6_z*sin(theta_3 + theta_4 + theta_5))*cos(theta_2);

    double temp_14 = (-config.l5_y*cos(theta_3 + theta_4) + config.l5_z*sin(theta_3 + theta_4) - config.l6_y*cos(theta_3 + theta_4 + theta_5) + config.l6_z*sin(theta_3 + theta_4 + theta_5))*cos(theta_2);

    double temp_15 = (-config.l6_y*cos(theta_3 + theta_4 + theta_5) + config.l6_z*sin(theta_3 + theta_4 + theta_5))*cos(theta_2);

    double temp_16 = 0;

    double temp_17 = 0;

    double temp_18 = 1;

    double temp_19 = 1;

    double temp_20 = 1;

    double temp_21 = 1;

    double temp_22 = 0;

    double temp_23 = 0;

    double temp_24 = 0;

    double temp_25 = 0;


    J_foot_combined << temp_1, temp_2, temp_3, temp_4, temp_5,
                                temp_6, temp_7, temp_8, temp_9, temp_10,
                                temp_11, temp_12, temp_13, temp_14, temp_15,
                                temp_16, temp_17, temp_18, temp_19, temp_20,
                                temp_21, temp_22, temp_23, temp_24, temp_25;
}

void update_J_foot_dot(double theta_1, double theta_2, double theta_3, double theta_4, double theta_5, double thetadot1, double thetadot2, double thetadot3, double thetadot4, double thetadot5, Eigen::Matrix<double, 3, 5> &J_foot_dot, leg_config &config) {

    double temp_1 = config.l2_x*thetadot1*cos(theta_1) - config.l2_y*thetadot1*sin(theta_1) + config.l3_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l3_x*thetadot2*sin(theta_1)*sin(theta_2) - config.l3_y*thetadot1*sin(theta_1) + config.l3_z*thetadot1*sin(theta_2)*cos(theta_1) + config.l3_z*thetadot2*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l4_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l4_y*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) + config.l4_z*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l5_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l5_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) + thetadot1*sin(theta_3 + theta_4)*sin(theta_1) + thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l6_x*thetadot1*cos(theta_1)*cos(theta_2) - config.l6_x*thetadot2*sin(theta_1)*sin(theta_2) + config.l6_y*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2)) + config.l6_z*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2));

    double temp_2 = (-config.l3_x*sin(theta_2) + config.l3_z*cos(theta_2) - config.l4_x*sin(theta_2) + config.l4_y*sin(theta_3)*cos(theta_2) + config.l4_z*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x*sin(theta_2) + config.l6_y*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*thetadot1*sin(theta_1) + (config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x*thetadot2*cos(theta_2) - config.l6_y*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_y*thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*cos(theta_1);

    double temp_3 = -config.l4_y*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l4_z*(-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l6_y*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2));

    double temp_4 = -config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l5_z*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l6_y*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2));

    double temp_5 = -config.l6_y*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) + config.l6_z*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2));

    double temp_6 = config.l2_x*thetadot1*sin(theta_1) + config.l2_y*thetadot1*cos(theta_1) + config.l3_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l3_x*thetadot2*sin(theta_2)*cos(theta_1) + config.l3_y*thetadot1*cos(theta_1) + config.l3_z*thetadot1*sin(theta_1)*sin(theta_2) - config.l3_z*thetadot2*cos(theta_1)*cos(theta_2) + config.l4_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l4_x*thetadot2*sin(theta_2)*cos(theta_1) - config.l4_y*(-thetadot1*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot1*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_3)*cos(theta_1)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_3) + thetadot3*sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l4_z*(-thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3) + thetadot1*sin(theta_3)*cos(theta_1) + thetadot2*cos(theta_1)*cos(theta_2)*cos(theta_3) + thetadot3*sin(theta_1)*cos(theta_3) - thetadot3*sin(theta_2)*sin(theta_3)*cos(theta_1)) + config.l5_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l5_x*thetadot2*sin(theta_2)*cos(theta_1) - config.l5_y*((thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1) + (thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) - config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot2*cos(theta_3 + theta_4)*cos(theta_1)*cos(theta_2)) + config.l6_x*thetadot1*sin(theta_1)*cos(theta_2) + config.l6_x*thetadot2*sin(theta_2)*cos(theta_1) - config.l6_y*((thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - thetadot1*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2)) - config.l6_z*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - thetadot1*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_1)*cos(theta_2));

    double temp_7 = -(-config.l3_x*sin(theta_2) + config.l3_z*cos(theta_2) - config.l4_x*sin(theta_2) + config.l4_y*sin(theta_3)*cos(theta_2) + config.l4_z*cos(theta_2)*cos(theta_3) - config.l5_x*sin(theta_2) + config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x*sin(theta_2) + config.l6_y*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z*cos(theta_3 + theta_4 + theta_5)*cos(theta_2))*thetadot1*cos(theta_1) + (config.l3_x*thetadot2*cos(theta_2) + config.l3_z*thetadot2*sin(theta_2) + config.l4_x*thetadot2*cos(theta_2) + config.l4_y*thetadot2*sin(theta_2)*sin(theta_3) - config.l4_y*thetadot3*cos(theta_2)*cos(theta_3) + config.l4_z*thetadot2*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot3*sin(theta_3)*cos(theta_2) + config.l5_x*thetadot2*cos(theta_2) - config.l5_y*(thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_2) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_2) + config.l5_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4) + config.l6_x*thetadot2*cos(theta_2) - config.l6_y*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_y*thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) + config.l6_z*thetadot2*sin(theta_2)*cos(theta_3 + theta_4 + theta_5))*sin(theta_1);

    double temp_8 = -config.l4_y*(thetadot1*sin(theta_1)*sin(theta_3) + thetadot1*sin(theta_2)*cos(theta_1)*cos(theta_3) + thetadot2*sin(theta_1)*cos(theta_2)*cos(theta_3) - thetadot3*sin(theta_1)*sin(theta_2)*sin(theta_3) - thetadot3*cos(theta_1)*cos(theta_3)) + config.l4_z*(-thetadot1*sin(theta_1)*cos(theta_3) + thetadot1*sin(theta_2)*sin(theta_3)*cos(theta_1) + thetadot2*sin(theta_1)*sin(theta_3)*cos(theta_2) + thetadot3*sin(theta_1)*sin(theta_2)*cos(theta_3) - thetadot3*sin(theta_3)*cos(theta_1)) - config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) + thetadot1*sin(theta_3 + theta_4)*sin(theta_1) + thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - config.l6_y*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2));

    double temp_9 = -config.l5_y*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) - (thetadot3 + thetadot4)*cos(theta_3 + theta_4)*cos(theta_1) + thetadot1*sin(theta_3 + theta_4)*sin(theta_1) + thetadot1*sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1) + thetadot2*sin(theta_1)*cos(theta_3 + theta_4)*cos(theta_2)) + config.l5_z*(-(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*cos(theta_1) + (thetadot3 + thetadot4)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4) + thetadot1*sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4) + thetadot2*sin(theta_3 + theta_4)*sin(theta_1)*cos(theta_2)) - config.l6_y*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2));

    double temp_10 = -config.l6_y*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) - (thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + thetadot1*sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1) + thetadot2*sin(theta_1)*cos(theta_3 + theta_4 + theta_5)*cos(theta_2)) + config.l6_z*(-(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*cos(theta_1) + (thetadot3 + thetadot4 + thetadot5)*sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + thetadot1*sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - thetadot1*sin(theta_1)*cos(theta_3 + theta_4 + theta_5) + thetadot2*sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*cos(theta_2));

    double temp_11 = 0;

    double temp_12 = -config.l3_x*thetadot2*sin(theta_2) + config.l3_z*thetadot2*cos(theta_2) - config.l4_x*thetadot2*sin(theta_2) + config.l4_y*thetadot2*sin(theta_3)*cos(theta_2) + config.l4_y*thetadot3*sin(theta_2)*cos(theta_3) + config.l4_z*thetadot2*cos(theta_2)*cos(theta_3) - config.l4_z*thetadot3*sin(theta_2)*sin(theta_3) - config.l5_x*thetadot2*sin(theta_2) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_2)*cos(theta_3 + theta_4) + config.l5_y*thetadot2*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*(thetadot3 + thetadot4)*sin(theta_3 + theta_4)*sin(theta_2) + config.l5_z*thetadot2*cos(theta_3 + theta_4)*cos(theta_2) - config.l6_x*thetadot2*sin(theta_2) + config.l6_y*(thetadot3 + thetadot4 + thetadot5)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5) + config.l6_y*thetadot2*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5)*sin(theta_2) + config.l6_z*thetadot2*cos(theta_3 + theta_4 + theta_5)*cos(theta_2);

    double temp_13 = (config.l4_y*cos(theta_3) - config.l4_z*sin(theta_3) + config.l5_y*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4) + config.l6_y*cos(theta_3 + theta_4 + theta_5) - config.l6_z*sin(theta_3 + theta_4 + theta_5))*thetadot2*sin(theta_2) + (config.l4_y*thetadot3*sin(theta_3) + config.l4_z*thetadot3*cos(theta_3) + config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4) + config.l6_y*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + config.l6_z*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2);

    double temp_14 = (config.l5_y*cos(theta_3 + theta_4) - config.l5_z*sin(theta_3 + theta_4) + config.l6_y*cos(theta_3 + theta_4 + theta_5) - config.l6_z*sin(theta_3 + theta_4 + theta_5))*thetadot2*sin(theta_2) + (config.l5_y*(thetadot3 + thetadot4)*sin(theta_3 + theta_4) + config.l5_z*(thetadot3 + thetadot4)*cos(theta_3 + theta_4) + config.l6_y*(thetadot3 + thetadot4 + thetadot5)*sin(theta_3 + theta_4 + theta_5) + config.l6_z*(thetadot3 + thetadot4 + thetadot5)*cos(theta_3 + theta_4 + theta_5))*cos(theta_2);

    double temp_15 = (config.l6_y*sin(theta_3 + theta_4 + theta_5) + config.l6_z*cos(theta_3 + theta_4 + theta_5))*(thetadot3 + thetadot4 + thetadot5)*cos(theta_2) + (config.l6_y*cos(theta_3 + theta_4 + theta_5) - config.l6_z*sin(theta_3 + theta_4 + theta_5))*thetadot2*sin(theta_2);

    J_foot_dot << temp_1, temp_2, temp_3, temp_4, temp_5,
                            temp_6, temp_7, temp_8, temp_9, temp_10,
                            temp_11, temp_12, temp_13, temp_14, temp_15;
}

void update_Lambda(Eigen::Matrix<double, 3, 5> &J_foot, Eigen::Matrix<double, 5, 5> &B, Eigen::Matrix<double, 3, 3> &Lambda) {
    // Lambda_t = np.linalconfig.g.inv(J_t.dot(B_t_inv).dot(J_t.T))
    Lambda = (J_foot * B.inverse() * J_foot.transpose()).inverse();
}

void update_orientation(double theta_1, double theta_2, double theta_3, double theta_4, double theta_5, double &phi, double &theta, double &psi) {
    phi = theta_3 + theta_4 + theta_5;
    theta = theta_2;
    psi = theta_1;
}

void update_Kp(Eigen::Matrix<double, 3,3> &Lambda, Eigen::Matrix<double, 3, 1> omega_desired, double &Kp_orientation, Eigen::Matrix<double, 5, 5> &Kp) {
    Kp << pow(omega_desired(0), 2) * Lambda(0, 0), 0, 0, 0, 0,
                    0, pow(omega_desired(1), 2) * Lambda(1, 1), 0, 0, 0,
                    0, 0, pow(omega_desired(2), 2) * Lambda(2, 2), 0, 0,
                    0, 0, 0, Kp_orientation, 0,
                    0, 0, 0, 0, Kp_orientation;
}

void update_Kd(Eigen::Matrix<double, 3,3> &Lambda, Eigen::Matrix<double, 3, 1> omega_desired, Eigen::Matrix<double, 3, 3> &h, double &Kd_orientation, Eigen::Matrix<double, 5, 5> &Kd) {
    Kd <<  2 * h(0, 0) * Lambda(0, 0) * omega_desired(0), 0, 0, 0, 0,
                    0, 2 * h(1, 1) * Lambda(1, 1) * omega_desired(1), 0, 0, 0,
                    0, 0, 2 * h(2, 2) * Lambda(2, 2) * omega_desired(2), 0, 0,
                    0, 0, 0, Kd_orientation, 0,
                    0, 0, 0, 0, Kd_orientation;
}

void update_tau_ff(Eigen::Matrix<double, 5, 1> &G, Eigen::Matrix<double, 5, 1> &C, Eigen::Matrix<double, 3, 5> &J_foot, Eigen::Matrix<double, 3, 5> &J_foot_dot, Eigen::Matrix<double, 3, 3> &Lambda, Eigen::Matrix<double, 3, 1> &accel_desired, Eigen::Matrix<double, 5, 1> &q_dot, Eigen::Matrix<double, 5, 1> &tau_ff) {
    //tau_ff = J_t.T.dot(Lambda_t).dot(a_des - J_dot_t.dot(q_dot_t)) + C_t + config.g_t
    tau_ff = G + C + J_foot.transpose() * Lambda * (accel_desired - J_foot_dot * q_dot);
}

void update_foot_pos(double theta_1, double theta_2, double theta_3, double theta_4, double theta_5, double &phi, double &psi, Eigen::Matrix<double, 5, 1> &foot_pos, leg_config &config) {
    double temp_1 = -config.l1_x - config.l2_x*cos(theta_1) + config.l2_y*sin(theta_1) - config.l3_x*cos(theta_1)*cos(theta_2) + config.l3_y*sin(theta_1) - config.l3_z*sin(theta_2)*cos(theta_1) - config.l4_x*cos(theta_1)*cos(theta_2) + config.l4_y*(sin(theta_1)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1)) - config.l4_z*(sin(theta_1)*sin(theta_3) + sin(theta_2)*cos(theta_1)*cos(theta_3)) - config.l5_x*cos(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4)) - config.l5_z*(sin(theta_3 + theta_4)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4)*cos(theta_1)) - config.l6_x*cos(theta_1)*cos(theta_2) - config.l6_y*(sin(theta_3 + theta_4 + theta_5)*sin(theta_2)*cos(theta_1) - sin(theta_1)*cos(theta_3 + theta_4 + theta_5)) - config.l6_z*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1) + sin(theta_2)*cos(theta_3 + theta_4 + theta_5)*cos(theta_1));

    double temp_2 = -config.l1_y - config.l2_x*sin(theta_1) - config.l2_y*cos(theta_1) - config.l3_x*sin(theta_1)*cos(theta_2) - config.l3_y*cos(theta_1) - config.l3_z*sin(theta_1)*sin(theta_2) - config.l4_x*sin(theta_1)*cos(theta_2) - config.l4_y*(sin(theta_1)*sin(theta_2)*sin(theta_3) + cos(theta_1)*cos(theta_3)) - config.l4_z*(sin(theta_1)*sin(theta_2)*cos(theta_3) - sin(theta_3)*cos(theta_1)) - config.l5_x*sin(theta_1)*cos(theta_2) - config.l5_y*(sin(theta_3 + theta_4)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4)*cos(theta_1)) + config.l5_z*(sin(theta_3 + theta_4)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4)) - config.l6_x*sin(theta_1)*cos(theta_2) - config.l6_y*(sin(theta_3 + theta_4 + theta_5)*sin(theta_1)*sin(theta_2) + cos(theta_3 + theta_4 + theta_5)*cos(theta_1)) + config.l6_z*(sin(theta_3 + theta_4 + theta_5)*cos(theta_1) - sin(theta_1)*sin(theta_2)*cos(theta_3 + theta_4 + theta_5));

    double temp_3 = -config.l1_z - config.l2_z + config.l3_x*sin(theta_2) - config.l3_z*cos(theta_2) + config.l4_x*sin(theta_2) - config.l4_y*sin(theta_3)*cos(theta_2) - config.l4_z*cos(theta_2)*cos(theta_3) + config.l5_x*sin(theta_2) - config.l5_y*sin(theta_3 + theta_4)*cos(theta_2) - config.l5_z*cos(theta_3 + theta_4)*cos(theta_2) + config.l6_x*sin(theta_2) - config.l6_y*sin(theta_3 + theta_4 + theta_5)*cos(theta_2) - config.l6_z*cos(theta_3 + theta_4 + theta_5)*cos(theta_2);


    foot_pos << temp_1,
               temp_2,
               temp_3,
               phi,
               psi;
}

void update_foot_vel(Eigen::Matrix<double, 5, 5> &J_foot_combined, Eigen::Matrix<double, 5, 1> &q_dot, Eigen::Matrix<double, 5, 1> &foot_vel) {
    foot_vel = J_foot_combined * q_dot;
}

void update_tau_setpoint(Eigen::Matrix<double, 5, 5> &J_foot_combined, Eigen::Matrix<double, 5, 5> &Kp, Eigen::Matrix<double, 5, 1> &pos_desired, Eigen::Matrix<double, 5, 1> &foot_pos, Eigen::Matrix<double, 5, 5> &Kd, Eigen::Matrix<double, 5, 1> &vel_desired, Eigen::Matrix<double, 5, 1> &foot_vel, Eigen::Matrix<double, 5, 1> &tau_ff, Eigen::Matrix<double, 5, 1> &tau_setpoint) {
    //tau_setpoint_t = J_t.T.dot(K_p.dot(p_des - p_t) + K_d.dot(v_des - v_t)) + tau_ff_t
    tau_setpoint = J_foot_combined.transpose() * (Kp * (pos_desired - foot_pos) + Kd * (vel_desired - foot_vel)) + tau_ff;
}