Momentum Control for Crane Load Stabilization

Modeling and Sizing of Control Moment Gyroscopes for Cranes

Von der Fakultät für Architektur der Rheinisch-Westfälischen Technischen Hochschule Aachen zur Erlangung des akademischen Grades eines Doktors der Ingenieurwissenschaften genehmigte Dissertation

Vorgelegt von: Daniel A. Haarhoff

Berichter:

Univ.-Prof. Dr. techn. Sigrid Brell-Cokcan

Univ.-Prof. Dr.-Ing. Robert Schmitt

Tag der mündlichen Prüfung: 13. Juli 2021

Acknowledgements

This work was made possible, supported and accompanied by many great people.

A massive thank you to Prof. Dr. Sigrid Brell-Cokcan for giving me this challenging project, for setting the bar high and trusting in me. You gave me incredible opportunities and room to learn and grow.

Thank you to all of my fellow researchers, colleagues and student assistants at our chair. Amazing amounts of talent and ideas – it was inspiring to work with you all. In particular I want to thank Elisa and Sven for being there through thick and thin. I will always remember fondly our many discussions and achievements.

Our chair would not be what it is without the amazing mix of people that got drawn to it and this project was supported by several of them. I will be forever indebted to Kaveh for making him suffer the many foibles of Simulink. He persevered and managed to get our prototype running. A prototype that would not exist without the great work of our student assistants Felix and Pascal. And most recently Davide joined us and I got to spend an incredibly insightful week handing over the project.

Thank you to my father for teaching me what a PhD is actually about and my mother for teaching me early on not to turn everything into a Doktorarbeit (I get it now). She also did an incredible job at proofreading and editing this work.

Thank you to the gladiators, whom one day I will defeat.

And above all, thank you Eva.

The original idea and subsequent work contained in this thesis have been submitted as a patent application [1]. Therefore many of the ideas and solutions in this thesis are part of that application, in particular the integrated approach of using CMGs to turn cranes into stable platforms for automation.

A theoretical validation of the fundamental approach was published as part of the MTM Robotics 2016 [2].

A note regarding formats

This dissertation is available as an HTML-file will all figures embedded, an EPUB ebook as well as a PDF. The recommended format is HTML as this lets the reader not only adjust the font size as needed, but also allows for full screen figures and animated GIFs that illustrate complex motion more vividly than a graph.

All three formats where generated from markdown files converted with Pandoc. The data, code for figure generation and content transformation as well a Dockerfile for the containerized build environment are available in a git repository.

Outline

The following is a documentation of efforts to understand whether it would be possible to operate a robot hanging from a crane. Given the desire of increasing automation in the construction industry the ability to extend the working area of conventional industrial robots by hanging them from cranes could be of great use and impact.

The introduction (chapter 1) begins with a summary of why construction robots are of interest, their current availability and abilities. Its second section elaborates why hanging robots from cranes is of interest. The stabilization approach chosen for this work i.e. control moment gyroscopes (CMGs) is briefly introduced before discussing alternative approaches. Finally the potential of the chosen approach is discussed.

With the focus of this work being CMG-based crane stabilization the main content begins with Chapter 2 introducing the fundamental principle of CMGs. The chapter goes on to discuss their use in spaceflight as well as terrestrial systems for maneuvering and stabilization. The idea to utilize gyroscopes to stabilize cranes is not new and so special attention is given to previous research in this field. This chapter thereby motivates this work by identifying areas not covered by existing research.

Following this, chapter 3 summarizes the existing theory on CMGs as it pertains to this work. The majority of this chapter is spent on introducing and explaining the dynamics of CMGs before moving on to the sizing and steering of them. The chapter thereby outlines the constraints of CMGs and limits of the existing models.

With the fundamentals of CMGs covered, chapter 4 develops the models required for assessing the core question of this work: how do the crane, robot and CMGs interact. The chapter begins by identifying and approximating the parameters of a crane that affect the crane-CMG-robot interaction. Following this a model is derived that approximates the crane and its load as a double pendulum. The load in this case being the CMGs, any kinematic systems and or any payloads hanging from them. The model is then further extended to include the dynamics of the CMG and robot as well as external forces in general.

Chapter 5 describes and structures the control problems of the crane-CMG system in its various applications. After an overview regarding crane control theory a simple controller for dampening oscillations is chosen and described.

Chapter 6 covers the experiments performed for with work. It begins with the validation of the double pendulum model’s behavior through simulations before evaluating the chosen dampening controller. The second section investigates the interaction of the dampening controller with the steering law of CMGs using a scissored pair CMG array as an example. Here the previously developed model allows for an analysis of how the different torques interact, which is of great importance when it comes to sizing consideration. The third section covers the generation of example forces and torques as generated by a robot’s motion. These forces and torques are used in the subsequent section to simulate the motion of a robot hanging from a crane and how the use of CMGs can reduce the deviation from the target path. The chapter closes with a discussion of the hardware prototype built in parallel to this work.

Building upon the insights gained from the the modelling and experimental chapters, chapter 7 develops a sizing methodology for the CMGs in a crane-CMG system. It begins by discussing how the sizing requirements and constraints arise from the various use cases of CMGs for crane stabilization. The chapter closes with a discussion of the relationships between the constraints and how the sizing for crane-CMG systems differs from other CMG applications.

The insights gained during this work are summarized in chapter 8 and used to identify the key challenges for further work.

Some specialized terminology is explained in a glossary (chapter 9) and the appendix (chapter 10) includes the python implementations of the models.

Construction robotics is a field heavily reliant on interdisciplinary collaboration. Therefore this thesis aims not only to be of use to engineers interested in dynamic systems and their control. Instead the intention is that, by covering certain fundamentals in greater detail, this work will be of use to architects, civil engineers and roboticists alike enabling them to understand the potentials and challenges of hanging a robot from a crane.

1 Introduction

1.1 The Need for Construction Robots

Construction is and will continue to face significant challenges: Our urban population is rising [3], necessitating not only the creation of significant amounts of new building stock, but also the densification and upgrading of existing stock. Furthermore we must reduce the impact on the environment of our buildings and their use. At the same time climate change has already resulted in increasingly extreme weather conditions and changes to the climate conditions that our buildings have to withstand. Many so called developed nations are also facing shortages in skilled construction workers [4], not only due to the aging population and unattractive working conditions, but also due to reliance on cheap foreign labour. The latter has caused many sectors in construction to stagnate technologically as innovation was more costly than masses of cheap labour [5].

Fortunately, technological advances have led other industries to develop means for greater individualization and increased flexibility in their production. Looking at earlier attempts at automating construction (particularly in Japan and Korea during the 80s), one can see how the technology of the day was unable to provide said individualization and flexibility required in construction [6]–[8]. With robots, sensors and fast computing becoming more ubiquitous, we are finally seeing the emergence of construction robotics.

First examples of these new on-site, commercially available robots are SAM100 the brick-laying robot [9] and Tybot the rebar-tying robot for bridge decks [10]. Robots have also entered the preproduction of buildings and we are slowly seeing more direct links between digital planning and individualized robotic execution. As industrial robots enter the construction industry, the existing construction machinery is also becoming more sophisticated. Dump trucks are already driving autonomously in open pit mines and excavators and other machines are being fitted with sensors and digital controls.

The availability of digitally-controllable machinery has always been key to increasing automation and individualization. Such machines are the bridge from digital planning to execution in the real world. As such it is wonderful to see the emergence of programmable machines suited for the construction site. Yet one aspect of the construction site is not addressed by the existing range of adapted industrial robots and upgraded construction machinery: the sheer size of construction sites. Therefore this work investigates the idea of hanging robots from cranes to increase their work envelope to that of the construction site.

1.2 Cranes as a Base for Robot Operations

The ability of cranes to provide logistics over large spans and at great heights has transformed construction since antiquity. They are the kinematic systems of choice for the large scale of construction sites, naval operations and large scale assembly tasks. However, the key to their success is also limits their ability as a provider of automated motion. Using a rope as the last link of their kinematic chain, cranes are able to cover height with a minimum of material. The flexibility of the rope also means that it requires extremely little space when retracted, as it can be coiled and wound around winches. Yet this flexibility also means that the rope cannot be used to push, only to pull. With the actors (the motors) of the crane connected to the end effector (the hook) via a flexible connection, it is difficult to produce controlled motion in the event of disturbances.

Figure 1: Overview of forces preventing stable operation of e.g. a robot suspended from a crane: a) center of gravity (CoG) of the hook/platform moves out of equilibrium, b) robot CoG gets moved out of equilibrium, c) forces and torques acting at robot base due to motion of robot, d) forces and torques stemming from interaction with external objects, e) external forces e.g. wind. Note that these all interact.

The ability of robots and other digitally controlled automation technology to perform accurate and repeatable movements is key to their usefulness. Hanging a robot from a crane to increase its work envelope needs to maintain this ability. The hypothetical assembly in Fig. 1 illustrates the various forces at play that could create disturbances, moving the crane’s hook, attached platform and hence the robot, leading to deviations in its intended movement.

To break down the various forces it helps to imagine conditions under which the platform would be stable and level in space. In a static case and without external forces these conditions would exist when the centers of gravity (CoG) of the platform and the robot below it are in line with the point of suspension for the rope i.e. the gantry. Any movement of the gantry would break this condition and lead to motion of the platform (a). Similarly, motion of the robot or attaching loads to it would change its CoG (b), once again leading to motion of the platform. Even if the CoG is maintained in an equilibrium position, motion of the robot results in forces and torques acting at the base of the robot (c), once again causing motion of the platform. Further disturbance of the platform equilibrium can be caused by the robot interacting with its environment (d) and the environment e.g. wind acting on the whole assembly (e).

With the platform being able to pivot and rotate freely around the hook and the hook itself being fairly free to move laterally the robot has a very unstable base of operations. It is obvious that by controlling the motion of the crane and robot it should be possible to reduce motion experienced by the platform. The control that can be exerted in this fashion is quite limited and poses a challenging control problem due to an underactuation with regard to the systems degrees of freedom (see Sec. 5.2). Such a control approach would also limit the ways in which the crane and robot could be used.

As such it would be advantageous to add additional means with which to control the platform, thereby giving the robot a more stable base of operations. The disturbances illustrated in Fig. 1 involve both forces and torques. This work focuses on adding controllable torque to a hypothetical platform without relying on physical constraints e.g. additional ropes or propulsion e.g. propellors. This means generating said torques in the middle of space.

Fortunately, spacecraft face a similar issues, having to maneuver without having land or air to push off from. Whilst spacecraft are usually known for their use of rocket and other propulsive devices, they also usually have means of producing torque without expending fuel. This feat is achieved through momentum control devices and this work will discuss the feasibility of using a specific type of these, called control moment gyroscopes (CMG), to enable construction robots for large work envelopes. Prior to introducing these devices and their existing uses (chapter 2) the following sections discuss alternative approaches to robotics for large workspaces and the potential applications for this work.

1.3 Alternative Approaches for Large Workspaces

A well studied approach for large workspaces are parallel tendon kinematics. Here, instead of a single rope positioning the end effector, multiple ropes/tendons are used. By spanning these tendons from different directions, it becomes possible to create stable positions and motions over large spaces. Such systems are commercially available for cameras in sports stadiums and researchers have proposed their use for a variety of other tasks.

The RoboCrane was originally a DARPA project at NIST that was proposed for several applications [11] including construction [12], [13]. Other systems such as the Fraunhofer Institute’s IPAnema have been aimed at logistics [14]. More recently there have also been further conceptual proposals for the use of parallel tendon robots on construction sites [15] as well as large scale 3D-printing efforts that utilize parallel tendon research prototypes [16] (see Fig. 2).

Figure 2: Rendering of a 3D-printing concept for construction sites [16]

Parallel tendon kinematics come with some challenges and disadvantages. Dealing with tendon elasticity is particularly relevant to vibrations in the system. Depending on the size of the end effector platform, the systems will also struggle to produce high torques, as these depend on the amount of leverage that the tendons have on the platform. Some researchers have therefore added momentum control devices to compensate torques in parallel tendon kinematics [17] (see Fig. 3), which point to interesting hybrid approaches utilizing parallel tendon systems in combination with CMGs. The bigger issue for construction sites will most likely be the need for the tendon’s winches to be placed around the workspace of the system. This means that tendons would be required to cross large areas of the construction site, which poses significant challenges with regard to collisions, setup costs as well as worker safety.

Figure 3: Experiments with dampening a parallel tendon platform using reaction wheels [17]. The left example is not using the reaction wheels. On the right, one can see them being used and rotating at the top of the platform.

Drones are the only system on a construction site with a workspace larger than cranes. They are therefore seeing use as an inspection, surveying and monitoring tool. They have also been proposed as a logistics and robotics platform for infrastructure (see [18] and Fig. 4). Given the weight of construction materials, drones’ small payload capabilities in comparison to cranes severely limits this approach. Their need to compromise flight time with battery weight is an additional constraint of drone based approaches. It seems relevant to point out that the building materials of some widely publicized drone-based constructions are foam blocks [19], nylon ropes [20], [21] and carbon fibers [22]. The use of propulsive systems (e.g. propellors) to rotate or stabilize crane loads has been mentioned in the literature and apparently there were experiments made in Japan or Korea [23], but sadly the results of these experiments are not available.

Figure 4: Excerpt from the 4th year report of the ARCAS project on collaborative flying robots for maintenance and construction. The consortium also performed experiments with a kerosine-fueled drone/helicopter capable of carrying a DLR LBR robot arm.

While mobile platforms in robotics are starting to enter industrial use, they are not able to provide the vertical reach of cranes. For small heights, linear axes can be added to the mobile platforms to increase the reach of their robot arms. Some have proposed attaching industrial robot arms to other systems to enhance their reach and construction site mobility. One example from research is the Digital Construction Platform by the MIT Media Lab [24] (Fig. 5). Others have attached articulated arms to hydraulic remote controlled demolition robots for use in nuclear decommissioning [25]. Apart from their vertical and horizontal reach, a further benefit of cranes can be their small footprint relative to their horizontal reach. Mobile platforms, on the other hand, would have to be provided with access via the ground.

Figure 5: The Digital Construction Platform that combines an industrial robot with a hydraulic arm to increase its reach. The red line has been drawn by the end of the hydraulic arm and the blue line by the small robot attached to it. [24]

1.4 Potential Applications

In comparison to the alternatives covered above, Fig. 6 illustrates the potential abilities of the proposed system of stabilizing a crane hook with momentum control devices. Generally speaking, it would add the ability to produce controlled torques at the hook of a crane without requiring any additional ropes or other kinematics. The control moment gyroscopes would allow for the compensation of the torques produced by a robot’s motion or other processes suspended from the hook. They would also provide control over the rotation of parts during transport and assembly operations. Furthermore they could assist in dampening crane oscillations.

These abilities could result in novel applications of cranes, not only on construction sites. Components could be moved with greater control for logistics and assembly operations. By increasing the control of the motion, safe automation of such tasks becomes possible. The ability to compensate process torques also makes it possible to use additional kinematic systems, be they specialized or general purpose (e.g. industrial robots). Examples of these applications are illustrated in Fig. 7.

Figure 6: Abilities of a CMG-stabilized crane. From left to right: process compensation, part rotation and dampening of pendulum oscillations.

Figure 7: Potential applications of a CMG-stabilized crane. From left to right: part transport with gantry cranes, transport and assembly of components on a construction site, mobile industrial robots.

2 Control Moment Gyroscope Applications

Control moment gyroscopes are an established technology in spacecraft design and have also seen some commercial terrestrial application. This chapter starts with a brief explanation of gyroscopic reaction torque before giving an overview of historic and current applications of momentum control devices in general and CMGs in particular. The applications have been split into three parts: spaceflight, general terrestrial and crane applications.

By investigating the various existing applications, this chapter hopes to provide a deeper understanding of the potentials and limitations of CMGs, to thereby motivate the subsequent work of this dissertation.

2.1 Gyroscopic Reaction Torque

The behavior of gyroscopes often runs counter to intuition due the relationship between torque, angular velocity and momentum. As it involves the cross product of their vectors, everything is constantly being rotated 90°.

Fundamental to understanding gyroscopes is that angular momentum (𝐡\boldsymbol{h}) is a conserved quantity i.e. it remains constant unless a torque (𝛕\boldsymbol{\tau}) is applied to the system. Furthermore it helps to remember that change in angular momentum is torque (𝛕=d𝐡dt\boldsymbol{\tau} = \frac{d\boldsymbol{h}}{dt}). When the applied torque is aligned with the angular momentum, the momentum simply changes in magnitude. This happens when a motor spins a wheel or we use our fingers to spin a top.

When the torque lies orthogonal to the angular momentum, we encounter the unintuitive side of gyroscopes. Since no component of the torque is in line with the angular momentum, its magnitude and hence the speed of the spin remain constant. Therefore, the change in angular momentum takes the form of a change in the orientation of the angular momentum. This is illustrated in Fig. 8.

Figure 8: Illustration of how a torque that lies orthogonal to an angular momentum causes a rotation of the angular momentum’s orientation instead of a change in its size. Here hh denotes the angular momentum and ω\omega is the rotational velocity that this vector is experiencing.

It is this change in orientation in the form of a rotation and its relation to the torque via the size of the angular momentum that lets us understand everything from simple toys (Fig. 9) to the momentum control systems of the International Space Station (Fig. 10).

Figure 9: Precession of a gyroscope. Gravity would usually cause the object to topple over. Instead, the conservation of angular momentum leads to a precession, whose motion causes a torque that balances out the torque caused by gravity.(Lucas Vieira, Public Domain)

Figure 10: Astronaut Dave Williams replacing one of the four dual gimbal CMGs on the ISS. (Public Domain, NASA Photo ID: S118-E-06998)

Before proceeding, I would like to emphasize three key points:

the angular momentum (𝐡\boldsymbol{h}) acts akin to a lever between 𝛚\boldsymbol{\omega} and 𝛕\boldsymbol{\tau}

the speed at which our gyroscope spins is not affected by torque acting orthogonal to its axis of rotation

such a torque will instead result in a rotation orthogonal to both angular momentum and said torque

The last point is essential to understanding CMGs where we do the inverse: rotate an angular momentum and thereby create a reaction torque. This is illustrated in Fig. 11.

Figure 11: Principle of a CMG. A spinning mass i.e. a gyroscope (Ω\Omega denoting its angular velocity) has an angular momentum (hh). By applying an angular velocity (ω\omega) orthogonal to the gyroscope’s spin axis we create a reaction torque (τ\tau) orthogonal to both the gimbal axis and the gyroscope’s spin axis.

The torque produced by such a gimbal rotation is described by:

𝛕=𝛚×𝐡(1) \boldsymbol{\tau} = \boldsymbol{\omega} \times \boldsymbol{h} \qquad(1)

Within a CMG the torque required to rotate the gimbal so as to produce a given rate 𝛚\boldsymbol{\omega} is independent of the angular momentum 𝐡\boldsymbol{h} of the gyroscope. The gimbal motor only has to overcome the rotational inertia of the gyroscope and the surrounding gimbal bearings etc. Yet since the torque of the CMG also depends on the angular momentum of the spinning gyroscope, we can increase the output torque of the CMG simply by increasing the spin speed of the gyroscope.

This is where the lever analogy is really appropriate: Just as a longer lever enables us to produce a greater output force with the same input force, so can a greater angular momentum (e.g. faster spinning gyroscope) enable us to produce a larger output torque with the same gimbal motor. Since the gyroscope’s motor only needs to overcome the bearing friction once it has reached the desired speed, CMGs enable us to produce very large torques with comparatively small motors. Leve et al. mention terrestrial systems capable of output in excess of 100,000 Nm that require only a few kWs to operate [26, p. 46]. This is quite remarkable when compared with e.g. a Tesla Model S motor which produces 1,250 Nm using 581 KW.

Unfortunately there is a caveat. Fig. 11 shows that the produced torque is applied to the body via the bearings of the gyroscope and those of the gimbal. This of course means that gimbals must be sized accordingly, but the greater issue occurs when the body containing the CMG happens to rotate around the axis of our output torque. In such a case the angular momentum lever becomes a problem, as it now leads to a large torque being put on the gimbal motor. Sizing the motor to accommodate for such cases can quickly negate the advantage provided by a CMG and this will be discussed in the later sizing sections (see Sec. 6.2.4).

2.2 Applications of CMGs

This section will cover a selection of CMG applications. Given their properties CMGs have a very narrow field of utility. With most of the research surrounding CMGs coming from space exploration, a brief historic review of CMGs in space will illustrate how the understanding of the fundamental principles of CMGs has evolved. The subsequent section covers the few terrestrial fields of application and serves to illustrate some of the challenges and limitations of CMGs. The final section covers prior work on stabilizing cranes with gyroscopes.

2.2.1 Development of CMG Technology and Spacecraft Application

The need to produce a torque without having something to push off from does not occur frequently. Given sufficient speed, even a plane can push off the air surrounding it. This explains why momentum control devices are mostly associated with space flight. Leve, Hamilton and Peck have summarized the work in this field in their book Spacecraft Momentum Control Systems [26].

For small spacecraft, reaction wheels are common and provide a straightforward means of transferring the torque of a motor to the spacecraft. Reaction wheels consist of a single motor attached to a disc or wheel. It is in larger craft with higher torque demands that the lever effect of CMGs becomes essential. The first CMGs to fly in space were three dual gimbal CMGs attached to Skylab. Dual gimbal CMGs are less efficient than single gimbal CMGs, as the torque produced is always transferred in part through a gimbal motor, whereas in a single gimbal design it will be transferred via the bearings. However there are several reasons why Skylab and many subsequent spacecraft including the ISS use dual gimbal CMGs.

As a gimbal rotates to produce torque with the gyroscope, so does the orientation of the output torque vector. This gives rise to complex steering laws and issues with singularities (See Sec. 9 for explanation of singularities). The Apollo program did not use CMGs, but did use three gyroscopes for inertial measurement of the spacecraft’s orientation. These gyroscopes were suspended in three nested gimbals, providing feedback regarding the spacecraft’s yaw, pitch and roll. The decision only to use three gimbals led to more frequent issues with gimbal lock (a singularity where the gimbal axis align) than was expected during multiple Apollo missions¹. Given the limited computational resources and understanding of CMG steering laws at the time, it is very understandable that NASA chose to go with dual gimbal designs, which eliminate the internal singularities on the CMG array.

The attitude control systems for the first spacewalks of the US program took a different approach. By mechanically linking two CMGs so that they mirror each other’s motion, one can create a scissored pair CMG array. Here the sum of the two gyroscope’s momentum vectors always lies along a single axis, making for much simpler steering (see also: Fig. 64 and Sec. 6.2.1). The disadvantage is that one has to use six, instead of three CMGs to be able to produce torque around three axes (i.e. three degrees of rotational freedom).

Over the years, several ways to array single gimbal CMGs and associated steering laws with singularity avoidance have been developed. Since redundancy is imperative in space flight, the arrays usually use four CMGs, unless other momentum control devices are also included in the spacecraft. The three common arrays for CMGs are box, pyramid and roof (see Fig. 12). Leve et. al recommend roof arrays due to their simpler and easier to avoid internal singularities.

Figure 12: Various configuration options for single gimbal CMGs [27]

Momentum control devices such as reaction wheels, CMGs or magneto-torquers² enable spacecraft to adjust and control their orientation without expending fuel. This makes them invaluable, given weight and volume constraints in space flight. However, these systems are also of value in attempts to add robot arms to spacecraft.

Whilst the ISS has a remote controlled arm, the control of this arm is not coupled with the attitude control system (ACS) of the ISS. The ACS simply continues to try and maintain the ISS in the desired orientation. This is possible due to the very large moment of inertia that the ISS possesses.

The only other robot arm to have operated in space is that of the ETS VII research satellite (see Fig. 13 and [28]). The goal of this research was to study how one satellite might capture another spacecraft for repairs or even just removal of space debris. The mission’s experiments were performed using both programmed motions as well as remote control operation, both with the speed limited to 2mm/s. Such low speeds were necessary to ensure that the ACS of the spacecraft could maintain its orientation. Note that such speeds are orders of magnitude below those of earthbound industrial robots.

Using the ETS VII satellite, Yoshida et al. were able to show the efficacy of reactionless motion planning using so called reaction-null-space [29]. These are sets of movement that produce zero reaction torque at the base of the robot. Such movements are very limited for conventional 6 DoF arms, making the approach suitable for arms with kinematic redundancy.

While the ETS VII is the only such system to fly in space, there has been a lot of research into robot arms in space (see [30] for a review), with the first robot arm flying as an experiment aboard the space shuttle Columbia [31]. Besides the above mentioned motion planning using reaction-null-space, other methods have been proposed to reduce the base reactions caused by the robot movement (see introduction of [32] for a good summary). These include the coordination of a second robot to compensate the torques of the first robot [33]. The authors show the efficacy of the approach, also with regard to energy consumption, but it must be assumed that adding a second arm just to be able to compensate the other would be less efficient than more powerful momentum control devices. Nevertheless, this work will be valuable if more than one robot is to be attached to a crane, especially as it would also be able to assist in maintaining the center of gravity. Other works have studied how one might coordinate the ACS and robot actuation to optimize their utilization while maintaining the satellite’s attitude as well as robot path accuracy [32], [34]–[36]. Others have proposed using CMGs as the actuators of the arm to create kinematics capable of moving in space without exerting torques on their host spacecraft [37]. To ensure accurate compensation and robot motion, methods for the identification of a systems inertial parameters after launch have been proposed [38].

Figure 13: Artists rendition of the ETS VII research satellite used to evaluate the use of robot arms to manipulate other satellites (bottom right, source: JAXA [39]). The satellite during ground tests (left, source: ESA Bulletin [40]) and overview of its robot setup (top right,[28])

2.2.2 Terrestrial Applications

Looking to terrestrial applications, we mostly see CMGs being used to stabilize vehicles. Attitude control with CMGs makes little sense on earth, as most vehicles need to rotate at velocities that are not feasible for CMG-based control, given the high reaction torques that would have to be sustained by the gimbal motors. As we have the ground, water and air to push off from, we usually can use wheels, fins and wings instead.

Therefore CMGs find terrestrial application where such systems do not work: two-wheeled vehicles that should remain upright at slow speeds or a standstill and roll stabilization for ships at rest. Early examples of both applications can be found. For ground vehicles there is a gyrocar commissioned in 1912 by Pyotr Petrovich Shilovsky who also built a monorail and wrote a book on the uses of gyroscopes in 1924 [41]. Ships of the same period saw the construction of probably the largest ever CMGs (regarding size), with the first deployment to a large passenger liner, the Conte di Savoia in 1931 [42]. The gyroscope weighed in 175t and spun at 910 rpm (see Fig. 14). Given that control engineering or cybernetics had not yet been established, these systems presumably operated on a passive control principle.

Figure 14: Gyro stabilizer aboard the Conte di Savoia passenger liner launched in 1934.

The way that such passive stabilization with gyroscopes works is as follows: The tipping vehicle exerts a torque orthogonal to the gyroscope’s momentum and the gimbal’s axis. This causes the gyroscope to precess around the gimbal axis, a movement that in turn causes a reaction torque counter to the tipping motion of the vehicle. If the gimbal resists the precession, for instance through friction, this torque also acts against the torque causing the excitation.

The low speeds of the motors available back then as well as the much simpler solution of adding further wheels or putting a foot on the ground are why gyroscopically stabilized ground vehicles did not become popular in the early 20th century. There have, however, been subsequent attempts at producing gyro cars.

In the late sixties a team around Thomas Summers, who had worked on guidance gyroscopes in the war, and the car designer Alex Tremulis, built the Gyro-X prototype. The goal was to increase road capacity and improve aerodynamics by slimming a car to two wheels, but the team went bankrupt before they were able to solve the engineering challenges involved in actually keeping the car stable. A recent startup called Lit Motors also attempted to produce a similar two-wheeled car and filed multiple patents [43] (see also Fig. 15). News surrounding the company went silent after an initial flurry of coverage around 2012 and some speculate that they were not able to reduce noise and vibrations sufficiently to create a usable consumer experience, but they have recently relaunched their [44]. For a discussion as well as a derivation of equations of motion and control systems for stabilization of two-wheeled vehicles see [45]. In this paper the authors also extend the equations of motion to use two gyroscopes of opposite spin direction and show how this benefits control performance.

Figure 15: Prototype of the Lit Motors C1 gyro-stabilized vehicle being driven on the webseries Spark produced by Maker Studios

Even though roll stabilizing gyroscopes went out of fashion for ships as hull and fin designs improved, they have become a commercial product offered by multiple companies (see Fig. 16 for an example). Their target market are yachts as well as commercial vessels, where roll stabilization at rest or slow speeds is of greater importance. The CMGs here are the largest CMGs commercially available today, since satellites require much lower torques. It is therefore unsurprising that a naval CMG was used in the recent resurrection of the Gyro-X at the Lane Motor Museum. It is also worth pointing out that some naval CMGs use the energy stored in the spinning gyroscope to provide additional electrical power during peaks in electrical load caused by the actuation. This was also considered for spacecraft, but as spacecraft batteries have reached a similar energy density to high-speed flywheels, the added weight of the required electronics and challenges of high velocity gyroscopes in space have led to this idea being discarded.

Figure 16: Marketing video for a ship-stabilizing gyroscope [46]. The left boat is being stabilized, the right one is not.

The gyroscopes used in naval CMGs are large enough to warrant a vacuum to reduce friction and are actuated using hydraulic systems. Filed patents point towards interesting engineering challenges [47], for instance regarding the cooling of the gyroscope bearings [48].

Given the passive stabilization principle discussed earlier, one might surmise that no actuation is required. The actuation is required to ensure that the gyroscope does not reach a singularity and also to limit the precession speed and thereby the output torque. Given the size of the gyroscopes and bearings involved, the actuation is also required to assist the gyroscope in overcoming friction. This is relevant during small excitations, where the friction might introduce a significant time delay in the system’s reaction.

Researchers have also proposed other applications for CMGs (see Fig. 17). The most exotic is probably the use of a CMG to stabilize a beating heart during surgery [49], [50]. Several researchers have also built CMGs into devices aimed at stabilizing human gait to e.g. assist the elderly [51], [52]. Others have suggested using CMGs to orient underwater robots to allow attitude control at zero velocity and with fewer thrusters (initially proposed in 2005 [53], further examples:[54]–[56]). Others have used reaction wheels instead of CMGs to create small, self-assembling robots that move by flipping themselves [57]. A quite well known example of momentum control is the Cubli, a self-balancing cube that also uses reaction wheels [58].

Figure 17: Other terrestrial applications proposed by researchers. From left to right: human gain stabilization [51], stabilizer for heart surgery [49], [50] and underwater robot [56].

2.2.3 Crane Applications

Given that there were significant efforts to automate construction in Japan and Korea dating back to the 1980s, it is unsurprising that research into the actuation of cranes has been done in these countries. The earliest literature relevant to cranes is from 1994 and concerned the stabilization of a passenger gondola. It uses a SPCMG with two 30kg flywheels to compensate wind excitation [59] (see Fig. 18). In 1997 a patent was filed on adding a motor to a crane hook to rotate the load [60]. In such a case the motor would have had to push off against the crane cable’s resistance to twisting.

A paper from 1998 describes field testing of a device using a single CMG [23] (see Fig. 19). The authors also mention previous work that utilized fans or propellors to actuate a crane’s load.³ The authors also analyzed how the device improved assembly times and reduced dangerous work. The greatest improvement was in tasks that involved the rotation of the load. Here the work duration was roughly halved and nearly all work they classified as dangerous was eliminated. Their device used the passive stabilization principle during transport and a remote controlled actuation to cause the load rotation.

Yi et al. have published several works on stabilizing beams during construction work (see Fig. 20) and [61]–[63]. Starting with a single CMG hanging from a single rope, their subsequent publications (one having only the abstract available in English) discuss the addition of an IMU since the rotary encoder attached to the single rope was too inaccurate. They also confront the same issue as naval CMGs, namely that slow rotations are not able to overcome the gimbal friction. Their last paper adds a clutch to the gimbal to avoid overloading the gimbal motor during passive stabilization. In [64] the authors use a motorized hook to rotate beams (see Fig. 21). The authors also filed two patents with regard to ways of powering such a hook, one proposed a clock spring with the other proposing to use the vertical hook travel to power the device. They incorrectly discuss [23], stating that the authors were only using passive stabilization. More recently researchers have once again used a single CMG to stabilize the yaw of a load attached to a model jib crane [65] (see Fig. 22). The authors run into difficulties with the singularity and changing orientation of the output torque.

Figure 18: CMG pair used to stabilize a gondola against wind excitation [59].

Figure 19: Single CMG device used in field tests [23].

Figure 20: Illustration of beam stabilization setup from [63].

Figure 21: Motorized crane hook experiments [64].

Figure 22: Single CMG experiments on a model jib crane [65].

The concept of using CMGs to rotate a load hanging from a crane has recently been turned into a commercially available product by the Australian company Verton. Founded in 2014 they published a patent regarding the rotational control of a crane load with gyroscopes in 2017 [66]. The product is called R-Series, was launched in 2019 and is currently available for 20 and 5 tonne loads. Their marketing material focusses on the increased worker safety and reduced handling times, as also covered in prior work by [23].

From the patent filing and limited information available on their website it appears that they use two single gimbal CMGs with the ability to add further CMGs as modules. The introduction of the patent discusses the issue of how reaction torque interferes with manual rotation of the load. Given the use of two gyroscopes I would assume they are using a scissored pair array. By rotating the two gimbals in the same direction (as opposed to mirrored) it should be possible to align the gyroscopes’ axes of rotation with that of the load without exerting any torque on it, thereby placing the device in a state that does not interfere with manual rotation.

Figure 23: The R-Series is a commercially available load rotation device that uses gyroscopes and was launched by the Australian company Verton in 2019. (Picture by Rod Pilbeam)

2.3 Summary

The idea of using gyroscopic effects for stabilization is an old one. Yet following this review of existing applications, it appears that a solution for the idea of stabilizing a robot hanging from a crane cannot easily be extrapolated from the existing applications of CMGs. Unlike a satellite or spacecraft we are predominantly interested in maintaining a given position, not in rotational agility. The oscillations and dynamic behavior of cranes will differ from those of ships, and unlike ships we have to deal not only with roll, but also pitch and yaw. It is this three-dimensional requirement that also sets this challenge apart from the previous work with CMGs and cranes.

The trajectory optimizations proposed for space robotics can most likely be adapted to our purpose, but cannot be applied directly due to the stark differences between spacecraft and crane dynamics. For instance, in space the torques produced by the robot will always only rotate the spacecraft, whereas attached to a crane we will also have to deal with translations caused by robot motions and other external forces.

We will have to develop an understanding of crane-CMG dynamics to allow us find a way to consolidate the different control and sizing requirements. On the one hand we have a system at rest being excited by e.g. a robot and on the other hand a system in an oscillatory state that needs to be dampened.

The Apollo guidance computer had to include functions to alert the astronauts to an impending gimbal lock and Mike Collins joked that all he wanted for Christmas was a fourth gimbal as he was repeatedly resetting the gimbals during Apollo 11.

Magneto-torquers rely on electro-magnets interacting with the earth’s magnetic field.

No English or German sources could be found that further detail this approach.

3 Theory of CMGs

This chapter covers various theoretical aspects of CMGs, starting with their dynamics. Subsequently a brief introduction is given regarding their steering, workspaces and sizing. The content of this chapter covers existing models and approaches, laying the groundwork for the following chapters that develop these aspects for the CMG-crane system.

3.1 Dynamics of CMGs

With the number of spinning parts involved in a CMG, the dynamics exhibit a certain complexity. To be able to size and control the CMGs for our application we need to understand and model these dynamics. This section will therefore introduce and discuss the model derived in great detail by Leve et al. [26, Ch. 4.5]. The resulting expression is the derivative (with respect to time) of the angular momentum of a CMG spacecraft with variable speed gyro-rotors [26]:

Nd𝐇dt=𝐉⋅Bd𝛚B/Ndt+(∑i=1nJg,iδ̈i𝐠̂i+Jr,i(Ω̇r,i𝐬̂i+δ̇iΩr,i𝐨̂i))+𝛚B/N×(𝐉⋅𝛚B/N+(∑i=1nJg,iδ̇i𝐠̂i+Jr,iΩr,i𝐬̂i))\begin{eqnarray} \frac{^N\text{d}\mathbf{H}}{\text{d}t} &=& \mathbf{J} \cdot \frac{^B\text{d}\mathbf{\omega}^{B/N}}{\text{d}t} \\ &+& \left( \sum^{n}_{i=1} J_{g,i} \ddot{\delta}_i \mathbf{\hat{g}}_i + J_{r,i} \left( \dot{\Omega}_{r,i} \mathbf{\hat{s}}_i + \dot\delta_i \Omega_{r,i} \mathbf{\hat{o}}_i \right) \right) \\ &+& \mathbf{\omega}^{B/N} \times \left( \mathbf{J} \cdot \mathbf{\omega}^{B/N} + \left( \sum^n_{i=1} J_{g,i} \dot{\delta}_i \mathbf{\hat{g}}_i + J_{r,i} \Omega_{r,i} \mathbf{\hat{{s}}}_i \right) \right) \end{eqnarray}

Leve et al. use vector-dyadic notation [26, Sec. 4.1] for the above equations. For a basic understanding it is best to disregard the dyadic notation, summations etc. and focus on the following symbols:

𝐇\mathbf{H}: angular momentum of the spacecraft

𝛚\mathbf{\omega}: angular velocity of the spacecraft

𝐉,Jr,Jg\mathbf{J}, J_r, J_g: inertia of spacecraft, gyroscope rotor and inner gimbal assembly

𝐬̂,𝐠̂,𝐨̂\mathbf{\hat{s}, \hat{g}, \hat{o}}: axis of the rotor, gimbal and output torque¹

δ̇,δ̈\dot{\delta}, \ddot{\delta}: angular velocity and acceleration of the gimbal

Ω\Omega: angular velocity of the gyroscopes rotor

Using this simplified notation let us look at the various parts contributing to the changes in angular momentum (for an exact discussion please refer to [26, Sec. 4.5]):

Rigid-Body Motion: all terms involving 𝐉\mathbf{J}

Gyroscopic Reaction Torques:
Caused by either the motion of the spacecraft or gimbal rotating an angular momentum vector.

JrΩδ̇𝐨̂J_r \Omega \dot{\delta} \mathbf{\hat{o}} \quad: Gimbal and spinning rotor

𝛚×(JrΩ𝐬̂)\mathbf{\omega} \times \left(J_r \Omega \mathbf{\hat{s}} \right) \quad: Spacecraft and spinning rotor

𝛚×(Jgδ̇𝐠̂)\mathbf{\omega} \times \left(J_g \dot{\delta} \mathbf{\hat{g}} \right) \quad: Spacecraft and rotation of gimbal assembly

Motor Torques:

Gimbal Motor

Gyroscope Motor

Depending on the required fidelity of the simulation and whether the model is used for sizing or control certain simplifications can be made:

For simplistic models one can separate 2a from the rest, as it represents the major output torque of single gimbal CMGs.

The torque exuded on the spacecraft by the gyroscope motor (3b) can be removed if the gyroscope’s velocity is constant during operation.

The gyroscopic reaction torque 2c will be much smaller than the other components and can therefore be disregarded for initial sizing etc.

Aside from providing the means to simulate the behavior of our CMGs, the parts 2b and 3a provide the requirements for gimbal motor sizing. Part 2a is the torque required to sustain the reaction torque stemming from the rotation of the spacecraft and part 3a is the torque required to provide the desired torque dynamics.

3.2 Steering of CMGs

The main challenge of steering CMGs lies in avoiding the complex singularities of a given array. Staying some distance away from them also reduces the accelerations required in the gimbals. Furthermore steering laws might also respect the gimbal limits regarding jerk, torque and speed. Alternatively they can leave this to the inner control loops of the gimbal.

Most arrays consist of at least four CMGs, permitting null space motion², that is gimbal motions that produce a net output torque of zero. Such motions can be used by the steering law to avoid singularities without introducing an error torque. Other steering laws actually enable passing through singularities and are categorized as singularity escaping. These methods introduce torque inaccuracies.

The fundamental principle of CMG steering laws lies in creating an inverse to the actuator Jacobian. That is, given the Jacobian i.e. the matrix describing the impact of the various gimbals’ rates on the output torque, its inverse provides us with a set of target gimbal rates needed to obtain the desired torque. Depending on the nature of the singularities, different approaches exist for the creation of a pseudo-inverse. Some solutions avoid singularities by simply forbidding the production of certain torques, reducing the workspace of the array to guarantee singularity-free operation.

One can also optimize solutions with a variety of methods and for a variety of goals e.g. torque accuracy, energy consumption or time required for a maneuver. Some of these optimizations might produce only local optima or not be able to guarantee torque accuracy between points in a torque trajectory. These optimizations are research field unto themselves. Leve et al. provide an an overview and selection of literature regarding CMG steering laws and their optimization [26, Ch. 7].

3.3 CMG Workspaces

One important concept with CMGs is their workspace, also referred to as the momentum envelope. A CMG array produces torque by changing the orientation of its momentum vector. This vector is the sum of the momentum vectors of the gyroscopes in the array. These individual momentum vectors of the gyroscopes maintain a constant magnitude, but; depending on the orientation; they might cancel each other out. Together they create a volume of possible momentum configurations of the array. This can be viewed as the workspace of the array whose axes are angular momentum around the three cartesian axes. Ergo, the dimensions of the envelope are in Nms.

The hull or envelope depends on the configuration of the array. If one imagines each CMG as having a momentum vector that can be gimbaled 360°, the result is a circle lying on the plane orthogonal to the gimbal’s axis. The workspace enclosed by the envelope is then the set of points defined by the sum of multiple such vectors as they are rotated.

To develop a more intuitive understanding of the momentum envelope, one can picture the gyroscopes being gimballed to move the momentum vector through the volume enclosed by the envelope. Remembering that a change in momentum is a torque, we can picture how the velocity of the point moving through the momentum space is the output torque of the array.

Therefore, want to produce a certain torque, the point begins moving in a certain direction. At some point the point reaches the envelope and the array simply cannot produce any more torque around that axis. The time this takes, i.e. the duration for which a given torque can be sustained, depends on the velocity of the point i.e. the magnitude of the torque and the size of the envelope. Therefore, if the velocity in momentum space is in Nm and the distance travelled corresponds to time, then it becomes understandable why this envelope is measured in Nms.

Note that close to singularities, the gimbals will have to move greater amounts to achieve the same travel in momentum space. This is akin to robots’ axes having to rotate further to travel a given distance in cartesian space when they are close to singularities. Hence, given a limited gimbal velocity and acceleration, the torque dynamics will change throughout the momentum envelope.

The envelope is not a convex hull i.e. it intersects itself and can have additional internal surfaces. These are the internal singularities that a steering law must avoid or be able to pass through them. See Fig. 24 for an illustration of singularities of different array types. Leve et al. also include a more in-depth discussion of the various types of singularities and how they are taken into account by different steering laws.

Figure 24: Momentum envelopes of different single gimbal CMG arrays, i.e. their inner and outer singularities. From left to right: box, roof and pyramid array [26]

The shape of the workspace is of relevance to this work as the requirements will not be uniform in all directions. A pendulum will often swing around a single axis, robot tasks might be dominated by torque around certain axes and part rotations will lie around the axis of the crane’s rope. Therefore it is of note that the shape the workspace of an array can be changed by altering its parameters. In a roof array, the critical parameter is the angle of the roof. See Fig. 25 and Fig. 26 for a comparison of different roof angles generated with the code provided in sec. 10.5.

Figure 25: Outer (green) and inner (orange) singularities for a four-CMG roof array with a roof angle of 45°.

Figure 26: Outer (green) and inner (orange) singularities for a four-CMG roof array with a roof angle of 30°.

3.4 Sizing of CMGs

Leve et al. provide a good overview regarding the sizing of spacecraft CMG [26]. For terrestrial systems there is little literature available, but in their paper on a wearable SPCMG Chiu et al. discuss their sizing process [51]. However, generally speaking, the main characteristics of a CMG array are:

the torque it can produce (Nm)

the agility of the torque production i.e. its acceleration (Nm/s)

the amount of torque it can sustain i.e. its momentum envelope (Nms).

The magnitude of these requirements obviously depends on the application for which the CMGs are being sized. This application will also pose constraints regarding torque accuracy, weight, size, vibrations and power consumption.

Looking at the sizing methods used for spacecraft, the requirements usually stem from the agility demanded of the spacecraft, also called slewrate. Given the moment of inertia of the spacecraft, the slewrate requirement will provide a required torque and duration that must be provided. The given slewrate also results in a base rate whose reaction torque must be handled by the gimbal motors. From this, the sizing of the CMGs can proceed and will subsequently be governed by the harsh constraints of spaceflight hardware.

This in particular is where the CMG sizing in this work diverges from spacecraft CMG design. Weight, size, power and computational resources are of much smaller concern when hanging the CMGs from a crane. For instance, some crane hooks have added weights which are required to ensure that the unloaded hook can overcome the rope friction in the pulley system. Furthermore, the slewrate or rather the rotation speed of loads is only one concern. The compensation of the robot’s motion and oscillations of the crane must also be taken into account. Hence, as already alluded to during the review of existing applications, an understanding of the crane-CMG-robot system is required. This is developed in the following chapter.

𝐨̂=𝐠̂×𝐬̂\mathbf{\hat{o}} = \mathbf{\hat{g}} \times \mathbf{\hat{s}}

For explanation of null space, error torque, Jacobian and pseudo-inverse see the glossary (chapter 9).

4 Modeling the Crane-CMG Application Systems

“The problem is, we don’t understand the problem.” – Paul MacCready

Two types of models are needed to develop sizing guidelines for the crane-CMG system. Their goals are, respectively:

derivation of abstracted inputs to the sizing

simulation of the designed system

Simulations of the system are needed to not only validate the sizing, but also to understand the system behavior.

The models need to cover three application scenarios for the CMG-crane system:

compensation of process torques

rotation of loads being lifted by the crane

dampening of the pendulum motion of the crane

Overall these models should thereby provide the means to abstract crane parameters to assist in the sizing of the CMGs. The requirements of the processes e.g. part rotation and robot motion also need to be obtained. Then, to link crane and application, a suitable model of the CMG system is required.

To understand the basic input parameters technical data provided by crane manufacturers is used. The process loads produced by a robot will be determined using real robot motions and a multi-physics simulation package. To understand the interaction of a CMG array with a crane it is modeled as a double pendulum system. The formulation of CMG dynamics (see Sec. 3.1) is applied to this model. Finally an interface for the application of external forces is created.

4.1 Parameter Space of Construction Cranes

Tower cranes play a central role in construction. This work therefore focusses on the parameter space of tower cranes. Fig. 27 shows a selection of tower cranes manufactured by Liebherr. Going from the compact, bottom-slewing crane, to one of the highest load cranes, two intermediate steps are also considered

Figure 27: Cranes used to estimate parameter space of construction cranes. From left to right (increasing payload): L1-24, 71 EC-B, 380 EC-B, 1000 EC-B.

Figure 28: Key parameters of the selected example cranes L1-24, 71 EC-B, 380 EC-B, 1000 EC-B.

From the data-sheets of these cranes the parameters relevant to the models can be obtained(see Fig. 28). The parameters that enable an estimation of the base rate of the CMGs induced by the motion of the crane are of particular interest, as this determines the magnitude of the gyroscopic reaction torque that the gimbal motors need to compensate (see Sec. 3.1). The translational movements of the hook/platform do not contribute to the base rate, but the period of the crane-rope/pendulum is of interest. Since the period of a pendulum depends on the length of the pendulum, the hook height is used as stand-in for the (maximum) pendulum length.

The oscillations of a crane tendon can be assumed to be sufficiently small to assume isochronism i.e. that the frequency remains constant while the amplitude changes. Whilst neither the amplitude of the oscillation nor the mass of the pendulum affect the frequency of the oscillation, they do affect the maximum angular velocity. It is possible to grasp this intuitively, if one considers the potential and kinetic energy of a pendulum.

In the zero crossing where the pendulum is aligned with gravity the potential energy is zero while the kinetic energy and hence the velocity is highest (see Fig. 29) When the pendulum is at its highest points, the inverse is true. As the potential energy is dependent upon the mass of the pendulum, the maximum payload of the cranes is added to the parameter list.

Figure 29: Illustration of the velocity and acceleration of a pendulum. CC-BY-SA, Wikipedia User Ruryk

An estimate of the amplitude of the crane oscillations can be derived from the maximum crane load, pendulum length and translational velocity caused by slewing the jib or moving the trolley. To determine the translational velocity caused by slewing, the jib length i.e. reach of the crane and the maximum load at the tip of the jib are also required:

vtrolley=NT/60vjib=NR2πr60ω=v⋅lhmax=mv22mgθmax=cos−1(l−hmaxl)\begin{align} v_{trolley} =& N_T / 60 \\ v_{jib} =& N_R \frac{2\pi r}{60} \\ \omega =& v \cdot l \\ h_{\text{max}} =& \frac{mv^2}{2mg} \\ \theta_{\text{max}} =& \cos{^-1}\left ( \frac{l-h_{\text{max}}}{l} \right ) \end{align}

With vv being the translational speed of the hook, NTN_T the trolley speed in m/min, NRN_R the jib’s rotational speed in rpm, rr the reach of the jib, ω=θ̇\omega=\dot\theta the angular velocity of the pendulum i.e. base rate of the hook, ll the length of the pendulum, hh the elevation of the hook relative to its lowest position and θ\theta the angle of the pendulum.

Figure 30: Base rates and maximum excitation of the selected example cranes L1-24, 71 EC-B, 380 EC-B, 1000 EC-B from the above derivations.

Note that all of the above assume a regular pendulum. As will be seen in the following section, it can be necessary to model the crane as a double pendulum. In such models, the hook and payload would be considered as separate masses. Note that some hooks are purposely heavy to overcome the rope-drag of the crane pulley when no load is attached.

The estimations derived here provide a worst case oscillation which is most likely in excess of physical oscillations in practice. The maximum values obtained above stem from a case where the crane is lifting the maximum load possible at the tip of the jib with the load at ground level. With such a load, the cranes rotates at maximum velocity and then stops completely.

4.2 Modeling the Crane

Whilst a CMG model will always have to be three-dimensional due to the rotation of the vectors involved, when it comes to cranes a 2D model will be presented before moving on to 3D models. A 2D model is not only easier to develop, but more importantly also easier to visualize, plot and understand. While simpler, it still helps to illustrate critical parts of the crane’s behavior and to develop control approaches. Often the insights from a lower dimensional model can help in the design of the higher dimensional ones.

For this case a 3D SPCMG model can be attached to a 2D crane, as the SPCMG produces its torque around a fixed axis. This illustrates the basic principles of the CMGs as they interact with the crane, while keeping the model relatively simple. It is also possible to study the effect of process torques and forces by constraining them to the 2D case.

This simplification was carried over to the hardware prototype. By building the crane as a swing that hangs from two diagonally spanned ropes, the motion can be mostly constrained to a single plane (see Fig. 31).

Figure 31: Sketch of the hardware prototype. The way it is suspended should ensure as close to 2D a motion as possible without having to resort to rigid links and large bearings.

The essence of a crane, a hook/load hanging from a rope, is a pendulum. However, in contrast to the basic point mass pendulum, a crane has several important differences. Most importantly the suspension point of the rope exhibits its own dynamics, causing excitation of the pendulum. These can stem from controlled movement of the trolley, jib or gantry but beyond this the entire crane structure often exhibits significant flex. Then the rope itself might actually be a set of ropes, is not rigid and also stretches under load. The load hanging from the hook of a crane is also often so large that it possesses significant inertia, leading to its own set of dynamic behavior that interacts with the crane. And finally, of course, there are external forces such as wind as well as drag and other dampening behavior.

This work makes several choices regarding what to include in the model. As the CMG acts on the crane by exerting torque on its payload, it is necessary to model the payload/CMGs as an additional distributed mass hanging from the crane. One of the applications is the dampening of pendulum motion of the crane. While this is most often induced by crane motions, nevertheless the pendulum’s suspension point will not be modeled as movable. Instead the assessment of the dampening capabilities will induce a pendulum motion through the initial conditions. The dynamics of the crane structure itself as well as the flexibility of the crane’s rope will also be ignored at this stage.

4.2.1 Review of Available Crane Models

A review of the existing crane models (see [67] for a review until 2003) supplies a large variety of models that focus on different aspects or crane types. It should be possible to copy, mix and adapt these to create a model suited for the current purpose. It also means that this CMG-focused model could be extended relatively easily. The following sections will develop this model from a two-dimensional double pendulum to a three-dimensional double pendulum and subsequently go from two point masses to a point mass and a distributed mass.

The Fig. 32 illustrates how the various parts of the crane, CMG, robot and load correspond to the parts of the model. The model ignores motion of the crane’s gantry/jib, and therefore the pendulum is suspended from a fixed point. The upper mass is a point mass that includes the mass of the hook and crane rope. The lower mass is a distributed mass that includes the CMGs, their platform and everything attached to it e.g. robot, load and other kinematics.

Figure 32: Correspondence of crane/CMG/load components to the model.

The generation of equations of motion and subsequent numerical integration are achieved using Python. The code builds on the educational example of Christian Hill [68],¹ the three-dimensional model extends the work of O’Connor and Habibi [69].

This work envisions that the developed model can be extended by others, not necessarily coming from engineering backgrounds. As such the following sections and appendix offer an extended discussion and the full source code.

4.2.2 2D Double Pendulum

This section uses the most basic double pendulum (massless rods with point masses, Fig. 33) as a starting point. For this the equations of motion are commonly known, see e.g. [68]:

Figure 33: 2D double pendulum as point masses on rigid, massless rods.

The Langrangian (ℒ=KE−PE\mathcal{L} = KE - PE) is the balance of potential energy (PE) and kinetic energy (KE) that describes our system:

ℒ=12(m1+m2)l12θ̇12+12m2l22θ̇22+m1l1l2θ̇1θ̇2cos(θ1−θ2)+(m1+m2)l1gcosθ1+m2gl2cosθ2\begin{align} \mathcal{L} = & \tfrac{1}{2}(m_1+m_2)l_1^2\dot{\theta}_1^2 + \tfrac{1}{2}m_2l_2^2\dot{\theta}_2^2 + m_1l_1l_2\dot{\theta}_1\dot{\theta}_2\cos(\theta_1 - \theta_2) \\ & + (m_1+m_2)l_1g\cos\theta_1 + m_2gl_2\cos\theta_2 \end{align}

From the Lagrangian the following equations of motion can be obtained using the Euler-Lagrange Equations.² This step can be performed using computer algebra systems (see code in Sec. 10.2.1).

0=ddt(∂ℒ∂q̇i)−∂ℒ∂qiθ̈1=m2gsinθ2cos(θ1−θ2)−m2sin(θ1−θ2)(l1θ̇12cos(θ1−θ2)+l2θ̇22)−(m1+m2)gsinθ1l1(m1+m2sin2(θ1−θ2))θ̈2=(m1+m2)(l1θ̇12sin(θ1−θ2)−gsinθ2+gsinθ1cos(θ1−θ2))+m2l2θ̇22sin(θ1−θ2)cos(θ1−θ2)l2(m1+m2sin2(θ1−θ2))\begin{align} 0 = & \frac{\mathrm{d}}{\mathrm{d}t}\left(\frac{\partial\mathcal{L}}{\partial \dot{q}_i}\right) - \frac{\partial \mathcal{L}}{\partial q_i} \\ \ddot{\theta}_1 = & \frac{{m_2g\sin\theta_2\cos(\theta_1-\theta_2) - m_2\sin(\theta_1 - \theta_2)(l_1\dot{\theta}_1^2\cos(\theta_1 - \theta_2) + l_2\dot{\theta}_2^2)}{ - (m_1+m_2)g\sin\theta_1}} {l_1(m_1 + m_2\sin^2(\theta_1-\theta_2))} \\ \ddot{\theta}_2 = & \frac{{(m_1+m_2)(l_1\dot{\theta}_1^2\sin(\theta_1-\theta_2) - g\sin\theta_2}{ + g\sin\theta_1\cos(\theta_1-\theta_2))+m_2l_2\dot{\theta}_2^2\sin(\theta_1-\theta_2)\cos(\theta_1-\theta_2)}}{l_2(m_1 + m_2\sin^2(\theta_1-\theta_2))} \end{align}

When these are solved through numerical integration (see code in Sec. 10.2.1) the familiar chaotic motion is obtained, see Fig. 34.

Figure 34: Chaotic motion of a double pendulum.

In the crane-CMG scenario, angles and velocities sufficient to create such motion outside of catastrophic failure events should not be encountered. Nevertheless the double-pendulum makes sense as a basis for the models for several reasons:

large payloads and their motions

CMGs control motion by exerting torque

The first point is obvious, but requires the extension of the model from a point mass to a distributed mass model (at least for the second mass i.e. the payload/CMG platform/robot). This will be covered in a later section since the stabilization and controlled movement of payloads is one of the goals of this work.

The second point can be illustrated by the point mass model. In Fig. 35 and Fig. 36 it can be seen how for small angles and velocities the double pendulum’s position is close to that of a regular pendulum. However, the interaction between the two parts of the pendulum can be seen by looking at the velocity and especially the accelerations. The impact of the lower pendulum can be expected to increase as the move is made to a distributed mass. As the CMGs exert torque on their platform, this impact of the lower link on the upper link will have an even greater effect.

Figure 35: Oscillations of a double pendulum at small angles and velocities.

Figure 36: Oscillations of a double pendulum at small angles and velocities showing a) how the position for such parameters comes close to a simple pendulum and b) how the two parts of the pendulum interact.

Until now the model has been using a point mass for both links. When the lower mass is replaced with a distributed mass, it is necessary to add a term to the kinetic energy to capture the rotational energy of the moving mass:

KWω=12Iω2\begin{equation} KW_{\omega} = \tfrac{1}{2} I \omega^2 \end{equation}

Since the point of rotation does not lie in the center of mass of our distributed mass (picture the platform hanging below the hook), the rotational inertia is not simply that of the mass. Instead the parallel axis theorem also known as Huygens-Steiner theorem must be applied:

I=ICoM+mr2\begin{equation} I = I_{CoM} + mr^2 \end{equation}

With ICoMI_{CoM} being the inertia of the mass around its center of mass, rr the distance from this center of mass to the axis of rotation and mm the mass of our object. These extensions are implemented in Sec. 10.2.2.

4.2.3 The 3D Model

Figure 37: Model of a point mass double pendulum in three dimensions with a fixed point of suspension.

If the point mass model is extended to three dimensions, two angles are required to describe the location of each point. In the 3D model of the double pendulum (Fig. 37) θi1\theta_{i1} are the polar angles and θi2\theta_{i2} the azimuthal angles. This model can be extended to include basic crane dynamics by making the x0x_0 and y0y_0 coordinates of the suspension point as well as l1l1 variable.

To convert to cartesian coordinates:

x0=0y0=0z0=0x1=l1⋅sin(θ11)⋅cos(θ12)y1=l1⋅sin(θ11)⋅sin(θ12)z1=−l1⋅cos(θ11)x2=x1+l2⋅sin(θ21)⋅cos(θ22)y2=y1+l2⋅sin(θ21)⋅sin(θ22)z2=z1−l2⋅cos(θ21)\begin{align} x_0 = & 0 \\ y_0 = & 0 \\ z_0 = & 0 \\ x_1 = & l_1 \cdot \sin(\theta_{11}) \cdot \cos(\theta_{12}) \\ y_1 = & l_1 \cdot \sin(\theta_{11}) \cdot \sin(\theta_{12}) \\ z_1 = & -l_1 \cdot \cos(\theta_{11}) \\ x_2 = & x_1 + l_2 \cdot \sin(\theta_{21}) \cdot \cos(\theta_{22}) \\ y_2 = & y_1 + l_2 \cdot \sin(\theta_{21}) \cdot \sin(\theta_{22}) \\ z_2 = & z_1 - l_2 \cdot \cos(\theta_{21}) \\ \end{align}

With this conversion the Langrangian can once again be formulated from the kinetic and potential energies:

PE=m1⋅g⋅z1+m2⋅g⋅z2KE=1/2⋅m1⋅ẋ12+ẏ12+ż12+1/2⋅m2⋅ẋ22+ẏ22+ż22ℒ=KE−PE\begin{align} PE = & m_1 \cdot g \cdot z_1 + m_2 \cdot g \cdot z_2 \\ KE = & ^1/_2 \cdot m_1 \cdot \dot{x}_1^2 + \dot{y}_1^2 + \dot{z}_1^2 \\ & + ^1/_2 \cdot m_2 \cdot \dot{x}_2^2 + \dot{y}_2^2 + \dot{z}_2^2 \\ \mathcal{L} = & KE - PE \end{align}

From this the Euler-Lagrange equations for θij\theta_{ij} can be obtained and subsequently solved for θ̇ij\dot{\theta}_{ij}. Given their complexity, this is done using a computer algebra system. See appendix Sec. 10.3 for the resulting equations and SymPy code used to obtain them.

The use of spherical coordinates to describe the kinematic constraints of the system leads to numerical issues during simulation. The issues arise due to the fact that the same coordinates can be arrived at by rotating the azimuthal angle by 180° while also flipping the sign of the polar angle (Fig. 38). While such jumps do not cause issues regarding the position of the pendulum, the spikes in angular velocity represent the kinetic energy in the system incorrectly. The effect of this can vary depending on the excitation/initial conditions of the simulation (see Fig. 94 and Fig. 96 in the appendix).

Figure 38: 3D double pendulum using spherical coordinates under small 2D excitation illustrating the issues of the use of spherical coordinates. Note how θi2\theta_{i2} jumps in steps of 180° causing θi1\theta_{i1} to remain negative. It also causes the spikes in angular velocity. These cause an erroneous dampening of the pendulum.

To alleviate this, the description of the kinematic constraints can be changed to use projected angles instead of spherical coordinates. This approach follows that of [69], where the authors derive the equations of motion for a double pendulum with an attached distributed mass that has two degrees of rotational freedom. The following extends this to a full three degrees of freedom (see Fig. 39), not only to model realistic crane load motion more closely but also to accommodate the load rotation use-case.

Figure 39: Model of a double pendulum in three dimensions with a fixed point of suspension and using projected angles instead of spherical coordinates. Note that the lower mass is now a distributed mass with three degrees of rotational freedom, while the upper mass is still a point mass with only two rotational degrees of freedom.

With the use of projected angles the cartesian expressions become:

x0=0y0=0z0=0x1=l1⋅sin(θ11)y1=l1⋅cos(θ11)⋅sin(θ12)z1=−l1⋅cos(θ11)⋅cos(θ12)x2=x1+l2⋅sin(θ21)y2=y1+l2⋅cos(θ21)⋅sin(θ22)z2=z1−l2⋅cos(θ21)⋅cos(θ22)\begin{align} x_0 = & 0 \\ y_0 = & 0 \\ z_0 = & 0 \\ x_1 = & l_1 \cdot \sin(\theta_{11}) \\ y_1 = & l_1 \cdot \cos(\theta_{11}) \cdot \sin(\theta_{12}) \\ z_1 = & -l_1 \cdot \cos(\theta_{11}) \cdot \cos(\theta_{12}) \\ x_2 = & x_1 + l_2 \cdot \sin(\theta_{21}) \\ y_2 = & y_1 + l_2 \cdot \cos(\theta_{21}) \cdot \sin(\theta_{22}) \\ z_2 = & z_1 - l_2 \cdot \cos(\theta_{21}) \cdot \cos(\theta_{22}) \\ \end{align}

Assuming a point mass pendulum, the Langrangian would be the same as before:

PE=m1⋅g⋅z1+m2⋅g⋅z2KE=1/2⋅m1⋅ẋ12+ẏ12+ż12+1/2⋅m2⋅ẋ22+ẏ22+ż22ℒ=KE−PE\begin{align} PE = & m_1 \cdot g \cdot z_1 + m_2 \cdot g \cdot z_2 \\ KE = & ^1/_2 \cdot m_1 \cdot \dot{x}_1^2 + \dot{y}_1^2 + \dot{z}_1^2 + \\ & ^1/_2 \cdot m_2 \cdot \dot{x}_2^2 + \dot{y}_2^2 + \dot{z}_2^2 \\ \mathcal{L} = & KE - PE \end{align}

Since the intention is to model the lower mass (the platform and load) as a distributed mass, the kinetic energy of the rotating mass has to be added to the Langrangian. For this it is necessary to express the rotational velocities and inertia of the mass in a common reference frame. This is defined as the center of gravity, hanging a distance l2l_2 from our point mass m1m_1. Should the center of gravity change due to e.g. robot motion, this difference will be modeled as an external torque acting upon the platform. The inertia tensor in this reference frame then is:

Im2=[Ixx000Iyy000Izz]X3Y3Z3\begin{align} I_{m2} = \begin{bmatrix} I_{xx} & 0 & 0 \\ 0 & I_{yy} & 0 \\ 0 & 0 & I_{zz} \\ \end{bmatrix} _{X_3Y_3Z_3} \end{align}

The rotations required to transform the reference frame X2Y2Z2X_2Y_2Z_2 that is aligned with the world axes to the reference frame X3Y3Z3X_3Y_3Z_3 also enable the rotational velocities ωX3Y3Z3\omega_{X_3Y_3Z_3} to be obtained from the projected angle velocities θ̇2j\dot{\theta}_{2j}.

Figure 40: Sequence of rotations to transform the world aligned reference frame to the reference frame of the distributed mass.

The rotations illustrated in Fig. 40 can be expressed as:

R2→3=RY(θ21)RX(θ22)RZ(θ23)=[cos(θ21)0−sin(θ21)010sin(θ21)0cos(θ21)][1000cos(θ22)−sin(θ22)0sin(θ22)cos(θ22)][cos(θ23)−sin(θ23)0sin(θ23)cos(θ23)0001]\begin{align} R_{2 \rightarrow 3} = & R_Y(\theta_{21})R_X(\theta_{22})R_Z(\theta_{23}) \\ = & \left[\begin{matrix}\cos{\left (\theta_{21} \right )} & 0 & - \sin{\left (\theta_{21} \right )}\\0 & 1 & 0\\\sin{\left (\theta_{21} \right )} & 0 & \cos{\left (\theta_{21} \right )}\end{matrix}\right] \left[\begin{matrix}1 & 0 & 0\\0 & \cos{\left (\theta_{22} \right )} & - \sin{\left (\theta_{22} \right )}\\0 & \sin{\left (\theta_{22} \right )} & \cos{\left (\theta_{22} \right )}\end{matrix}\right] \left[\begin{matrix}\cos{\left (\theta_{23} \right )} & - \sin{\left (\theta_{23} \right )} & 0\\\sin{\left (\theta_{23} \right )} & \cos{\left (\theta_{23} \right )} & 0\\0 & 0 & 1\end{matrix}\right] \end{align}

Thus the required rotational velocities can be expressed as follows. Note that -θ21\theta_{21} is used, as its rotation direction runs counter to the right-hand rule.

ω3=R2→3ωX2+R2→3ωY2+ωZ3=R[θ̇2200]+R[0−θ̇210]+[00θ̇23]ωX3=−θ̇21(−sinθ21sinθ22cosθ23−sinθ23cosθ21)+θ̇22(−sinθ21sinθ22sinθ23+cosθ21cosθ23)ωY3=−θ̇21cosθ22cosθ23+θ̇22sinθ23cosθ22ωZ3=−θ̇21(−sinθ21sinθ23+sinθ22cosθ21cosθ23)+θ̇22(sinθ21cosθ23+sinθ22sinθ23cosθ21)+θ̇23\begin{align} \omega_3 = & R_{2 \rightarrow 3} \omega_{X_{2}} + R_{2 \rightarrow 3} \omega_{Y_{2}} + \omega_{Z_{3}} \\ = & R \begin{bmatrix} \dot\theta_{22} \\ 0 \\ 0 \end{bmatrix} + R \begin{bmatrix} 0 \\ -\dot\theta_{21} \\ 0 \end{bmatrix} + \begin{bmatrix} 0 \\ 0\\ \dot\theta_{23} \end{bmatrix} \\ \omega_{X_{3}} = & -\dot{\theta}_{21} ( -\sin{\theta_{21}} \sin{\theta_{22}} \cos{\theta_{23}} - \sin{\theta_{23}} \cos{\theta_{21}} ) \\ & + \dot{\theta}_{22} (-\sin{\theta_{21}} \sin{\theta_{22}} \sin{\theta_{23}} + \cos{\theta_{21}} \cos{\theta_{23}})\\ \omega_{Y_{3}} = & -\dot{\theta}_{21} \cos{\theta_{22}} \cos{\theta_{23}} \\ & + \dot{\theta}_{22} \sin{\theta_{23}} \cos{\theta_{22}}\\ \omega_{Z_{3}} = & -\dot{\theta}_{21} (-\sin{\theta_{21}} \sin{\theta_{23}} + \sin{\theta_{22}} \cos{\theta_{21}} \cos{\theta_{23}}) \\ & + \dot{\theta}_{22} (\sin{\theta_{21}} \cos{\theta_{23}} + \sin{\theta_{22}} \sin{\theta_{23}} \cos{\theta_{21}}) \\ & + \dot{\theta}_{23} \end{align}

The Lagrangian then becomes:

ℒ=(KE+KEω)−PEKEω=1/2⋅ω3⋅Im2⋅ω3=1/2(IXX⋅ωX32+IYY⋅ωY32+IZZ⋅ωZ32)\begin{align} \mathcal{L} = & (KE + KE_\omega) - PE \\ KE_\omega = & ^1/_2 \cdot \omega_3 \cdot I_{m2} \cdot \omega_3 \\ = & ^1/_2 ( I_{XX} \cdot \omega^2_{X_3} + I_{YY} \cdot \omega^2_{Y_3} + I_{ZZ} \cdot \omega^2_{Z_3}) \\ \end{align}

The equations of motion can then once again be obtained using a computer algebra system. Since the 3D plotting of Matplotlib is slightly limited, the following animation is made using Rhino/Grasshopper (Fig. 41). For this the state-vectors of the simulation are transformed to cartesian points for the masses as well as an X and Y vector for the reference frame of our mass. See the appendix (Sec. 10.4) for equations of motion and code.

Figure 41: Visualization of simulated 3D double pendulum with the lower mass as a distributed mass. The lower mass begins with a small angular velocity around the link axis. Visualized using Rhino/Grasshopper.

This process can be continued to add further aspects such as wind or a movable point of suspension. The following section covers how such aspects can be added relatively easily by modifying the state of the system. Other aspects such as a movable point of suspension would require a new Langrangian, as they introduce new terms to the kinetic or potential energy.

An alternative would be to use multi-body simulation tools as are available in Modelica or Simulink. Here, individual blocks that contain e.g. a distributed mass or a rotary joint can be connected with each other (see Fig. 42).

Figure 42: Rudimentary implementation of this model in Modelica. Note that this lacks connections to input torque from the CMGs and uses a different angle description.

4.3 Adding External Torques

The torques and forces acting upon the system cannot be included in the above Langrangian as energies, instead they must be added as source terms to the resulting equations of motion.

The torque generated by the CMGs (τ\tau) changes the angular acceleration of the lower link depending on its moment of inertia. For the two-dimensional model, the necessary modifications to the equations of motion are straightforward:

θ′̈1=θ̈1θ′̈2=θ̈2+τm2l22\begin{align} \ddot{{\theta}'}_1 = & \ddot{\theta}_1 \\ \ddot{{\theta}'}_2 = & \ddot{\theta}_2 + \frac{\tau}{m_2 l_2^2} \end{align}

The three-dimensional model is slightly more complex due to the choice of angles. The torque produced by the CMGs is expressed in the reference frame of the distributed mass. The torque τZ\tau_{Z} is aligned with the link and therefore with the axis of rotation for θ23\theta_{23}, ergo having no impact on θ̈2[12]\ddot{\theta}_{2[12]}. To obtain θ̈2[12]\ddot{\theta}_{2[12]} first τ[XY]\tau_{[XY]} is transformed into the reference frame XYZ2XYZ_2 using the inverse of the rotation used above. The results are then projected onto X2X_2 and Y2Y_2 to obtain θ̈22\ddot{\theta}_{22} and θ̈22\ddot{\theta}_{22} respectively. Once again note the sign change for θ21\theta_{21}.

R3→2=R2→3TdetR3→2T=1, making it a valid rotationτXYXYZ2=R3→2[τX00]+R3→2[0τY0]θ̈21′=θ̈21−τXYXYZ2[010]⋅IYY−1θ̈22′=θ̈22+τXYXYZ2[100]⋅IXX−1θ̈23′=θ̈23+τZ\begin{align} R_{3 \rightarrow 2} = & R_{2 \rightarrow 3}^T \\ \operatorname{det}R^T_{3 \rightarrow 2} = & 1 \text{, making it a valid rotation} \\ \tau^{XYZ_{2}}_{XY} = & R_{3 \rightarrow 2} \begin{bmatrix} \tau_{X} \\ 0 \\ 0 \end{bmatrix} + R_{3 \rightarrow 2} \begin{bmatrix} 0 \\ \tau_{Y} \\ 0 \end{bmatrix} \\ \ddot{\theta}_{21}' = & \ddot{\theta}_{21} - \tau^{XYZ_{2}}_{XY} \begin{bmatrix} 0 \\ 1 \\ 0 \end{bmatrix} \cdot I_{YY}^{-1} \\ \ddot{\theta}_{22}' = & \ddot{\theta}_{22} + \tau^{XYZ_{2}}_{XY} \begin{bmatrix} 1 \\ 0 \\ 0 \end{bmatrix} \cdot I_{XX}^{-1} \\ \ddot{\theta}_{23}' = & \ddot{\theta}_{23} + \tau_{Z} \end{align}

The application of the CMG torques can then be added to the model as follows.

θ̈21′=θ̈21−(τX(−sinθ21sinθ22sinθ23+cosθ21cosθ23)+τY(sinθ23cosθ22))θ̈22′=θ̈22+(τX(−sinθ21sinθ22cosθ23−sinθ23cosθ21)+τY(cosθ22cosθ23))θ̈23′=θ̈23+τZ\begin{align} \ddot{\theta}_{21}' = & \ddot{\theta}_{21} - \left( \tau_{X} \left(-\sin{\theta_{21}} \sin{\theta_{22}} \sin{\theta_{23}} + \cos{\theta_{21}} \cos{\theta_{23}}\right) + \tau_{Y} \left(\sin{\theta_{23}} \cos{\theta_{22}} \right ) \right)\\ \ddot{\theta}_{22}' = & \ddot{\theta}_{22} + \left( \tau_{X} \left( - \sin{\theta_{21}} \sin{\theta_{22}} \cos{\theta_{23}} - \sin{\theta_{23}} \cos{\theta_{21}} \right ) + \tau_{Y} \left ( \cos{\theta_{22}} \cos{\theta_{23}} \right ) \right )\\ \ddot{\theta}_{23}' = & \ddot{\theta}_{23} + \tau_{Z} \end{align}

This encompasses all torques produced by the CMG, thereby cleanly separating the crane and CMG models and simplifying their respective development.

4.4 Adding External Forces

Following the above description of the torques, the forces will now be described. These comprise the forces from the robot’s motion, process forces or disturbances such as wind. Given the way the equations of motion are constructed around the angular motion of the links, the external forces need to be translated into torques. Subsequently they can be added to the other external torques.

A force acting on a pendulum will cause both a translational acceleration of its center of mass as well as a rotational acceleration around it. The magnitude of the translational acceleration depends on the force and mass of the pendulum. The rotational acceleration also depends on where the force is acting in relation to the center of mass. This leads to the interesting phenomenon of the center of percussion (see Sec. 9 for explanation).

In this case, where a double pendulum is being used, this means that there is an additional torque acting upon the lower link that depends on the force and its distance to the center of mass of the lower link. The force acts on the pivot point of the lower link and thereby on the upper link. Here it once again results both in an angular as well as translational acceleration. As the pivot of the upper link is assumed to be fixed and is connected by a rope, the translational acceleration has no impact. So for the 2D model the equations of motion can be extended as follows:

θ′̈1=θ̈1+F1xl1m1l12θ′̈2=θ̈2+τ+F2xbI2\begin{align} \ddot{{\theta}'}_1 = & \ddot{\theta}_1 + \frac{F_{1x} l_1}{m_1 l_1^2}\\ \ddot{{\theta}'}_2 = & \ddot{\theta}_2 + \frac{\tau + F_{2x} b}{I_2} \end{align}

bb being the distance between the force F2xF_{2x} and the center of mass of the lower link. The moment of inertia for the upper link is written explicitly, as it is always modeled as a point mass. The moment of inertia for the lower link depends on whether it is being modeled as a point or distributed mass.

Defining the external force as acting in the reference frame of the lower link, the forces F1xF_{1x} and F2xF_{2x} are the components of the external force that act perpendicular to the respective links. The force acting upon the upper link is then dependent upon θ1\theta_1 and θ2\theta_2 (Fig. 43):

F1x=F2xcos(θ1+θ2)+F2ysin(θ1+θ2)\begin{equation} F_{1x} = F_{2x} \cos(\theta_1 + \theta_2) + F_{2y} \sin(\theta_1 + \theta_2) \end{equation}

It is important to note that the forces acting upon the system due to gravity are already taken into account by the equations of motion derived from the Langrangian.

Figure 43: Illustration of the external force acting upon the 2D model

4.5 Payload Inertia

The payload inertia is of relevance to all three applications previously outlined. For the models a set of example inertia derived from the parameter space set forth by the selected example cranes will therefore be created (see Sec. 4.1).

For each crane a slab of concrete will be modeled, the weight of which matches the maximum load of the crane. The proportions of the slabs will be constant at 5, 0.1, 2 in X, Y, Z respectively. The density of concrete is assumed to be 2,400 kg/m3^3. Since the center of gravity of the payload will not lie in the point of rotation (the hook), the following assumes an offset equal to the size in Z. Using the parallel axis theorem, this results in the inertia listed in Fig. 44.

Figure 44: Example payload inertia assuming constant density (2,400 kg/m3^3), proportions (5, 0.1, 2) and a weight stemming from the max load of the associated crane. It is also assumed that the CoG is to be offset by the Z-size from the point of rotation.

4.6 Process Torques and Forces

Given the initial motivation of this work to stabilize an industrial robot hanging from a crane, a six-axis serial link robot is used as the model for a process generating torques and forces to be compensated by the CMGs. Since a robot is a generic motion provider that can provide a wide set of movements, several paths were created in an attempt to provide a representative set. These paths are:

random motion in a plane below the robot

vertical rectangle next to the robot

points next to the robot incl. approach and retraction

See Fig. 45 for animations of the toolpaths. These should roughly correspond to positioning/compensation, a continuous task (e.g. spray-painting) and a joining task (e.g. nail gun) respectively. The tasks were programmed using the Rhino/Grasshopper plugin KUKA|prc for a small KUKA KR3 R540 industrial robot (see Fig. 46). Since the programs are parametric they can easily be scaled to larger robots.

The KUKA|prc plug-in can output the required axis values for a programmed path. This inverse kinematic simulation is useful for solving singularity issues in the paths but does not limit the axis accelerations. This is obvious when looking at the axis values produced by KUKA|prc in Fig. 47, which have very sharp corners where the robot changes direction.

While more realistic robot simulation packages exist, for this project real axis values can be used to simplify matters. These can be obtained using the mxAutomation option of the plugin-in, mxAutomation being a protocol intended for interfacing programmable logic controllers (PLC) with KUKA controllers. The KUKA|prc software uses this protocol via UDP over a conventional Ethernet connection with the controller. Using mxAutomation, the robot path can be streamed to the controller and actual axis values are sent back. Time-stamped recordings of these can be used as input for a multi-body simulation.

The multi-body simulation is set up using the Simmechanics package in Simulink. The CAD files from the robot manufacturer are imported into SolidWorks and rotational joints added to the assembly. From this a Simmechanics file can be exported using the export plug-in provided by Mathworks and subsequently adapted to receive the recorded axis values and output the torques and forces experienced at the robot base (see Fig. 48). The inertia of the robot’s axes are estimated by distributing the robot’s mass according the volume of each link, which assumes a homogeneous density of the robot.

Figure 45: Three robot paths used in these experiments.

Figure 46: Parametric robot path programmed using the KUKA|prc plugin for Rhino/Grasshopper.

Figure 47: Axis values simulated by KUKA|prc. Note the sharp corners resulting from unlimited acceleration/jerk values making these values ill-suited to simulate the forces and torques at the robot base.

Figure 48: Body simulation of the KR3. The masses of the axes have been estimated from the total mass of the robot and the volume of the respective axes.

Also available at his website.

See the Wikipedia entry and lectures by Michel van Biezen for a deeper introduction.

5 Controller Design

To design a control system for this crane-CMG robot system, first the flow of information will be described for the three application scenarios previously mentioned. The interdependencies of the systems are analysed to create an integrated control approach. Finally existing dampening control strategies for cranes are discussed and one suited to the selected for evaluation.

5.1 Overall Flow of Information

These are the three applications in ascending order of control complexity:

part rotation

dampening

process compensation

Figure 49: Control flow for part rotation

In Fig. 49 there is a simple control loop for rotating a part with the CMGs, assuming that the crane is standing still. The desired state is a certain rotational position around the yaw-axis of the platform/load. This would be the case e.g. when the rotation of the part is a programmed action. Alternatively a certain rotational velocity could also be used as the desired state (θ23̇\dot{\theta_{23}}). This would be the case in a remote control scenario, where an operator would control the rotation manually. The controller sets a desired torque (τW\tau_W) which the CMG controller tries to achieve. The resulting torque (τY\tau_Y) then affects the pendulum resulting in the pendulum state PYP_Y. Any difference (PEP_E) from this is measured by the sensor (PMP_M) and fed as input into the controller.

As also pointed out in [64], the inertia of the part to be rotated obviously has a major impact on the torque required to perform the rotation. Therefore this control loop utilizes acquired knowledge regarding the torque output of the CMGs and the measured rotational acceleration to estimate the inertia of the part. This should improve the performance of the controller when used for programmed rotations but also ensure a consistent remote control experience for the operator, as their speed control will be similar, regardless of the part being handled.

Figure 50: Control flow for pendulum dampening as well as rotation of parts.

If one no longer assumes that the crane is standing still, the next matter to consider is dampening control loop (Fig. 50). This loop is actually nearly identical to the rotation control loop, the only difference being that the desired state of the pendulum (PWP_W) is extended so that all angles are zero, except for θ23\theta_{23}, which is set to the desired rotation of the part. The challenge in this loop is finding a suitable controller as, unlike the rotational control loop, these CMGs do not have direct control over all state variables. If the crane is modeled as a double pendulum, the loop is underactuated due to the CMGs only having indirect control of the state of the upper link. This will be covered in greater detail in Sec. 5.3. It will be seen in the experiments performed with the physical prototype that sensor noise and delays in the actuation of the CMGs add further challenges.

As the rotational inertia previously mentioned also scales the impact of our CMG torques during dampening, it becomes clear that an estimation of the rotational inertia is beneficial to any dampening control.

Figure 51: Control flow for the compensation of process torques

Finally Fig. 51 shows the more complex flow of information for the compensation of process torques. At its base lies the dampening/rotation controller, which is needed to dampen any oscillations that result from torques and forces that the CMG was not able to compensate.

In addition to the desired pendulum state PWP_W, there is now also a desired robot state RWR_W. The robot could be an actual industrial robot or any other form of kinematic attached to the CMG platform. The robot has its own motion controller that outputs motion commands (RSR_S). These cause the robot to move, which results in forces (FYF_Y) and torques (τY\tau_Y) that are experienced by the platform and hence the pendulum.

The torque created by the robot acts in addition to the torque created by the CMGs. Therefore the robot’s motion commands are also piped into a model of the robot that simulates the torques and forces produced at the base of the robot. The forces acting at the base of the robot result in additional torques to the lower link, which can also be fed forward to the dampening controller. This value is subtracted from the torque target set by the dampening controller and the CMGs should therefore ideally fully compensate any torque produced by the robot.

The torque on the upper link that results from the forces acting at the base of the robot cannot be directly compensated using the CMGs. The only way to counteract the effect of this torque is to cause a rotation of the lower link. Therefore rotation and dampening controller ought to be extended, with goal being to balance the error produced by the upper link with the error caused by the lower link moving to dampen said error in the upper link.

Since the real CMGs will not resemble their idealized model counterparts and in any case cannot fully compensate the impact of external forces, it has to be assumed that the robot’s motion will cause the pendulum to swing. This is why the measured position of the robot (RMR_M) and pendulum (PM′P_M') are fed into a second simple model that provides an actual position of the robot end effector (RM′R_M'). This position is subtracted from the target position coming from the path planning, so that the robot controller may attempt to compensate the error. Additional sensors could also be added to measure directly the error of the robot end effector. Since part of the torque at the robot’s base stems simply from the pull of gravity on the robot’s joints, it is also necessary to pipe the current platform angle into the robot model.

Once this is applied to an actual crane, the state has to be extended to include the position of the crane hook in addition to its orientation. A central controller would then set targets for both the cranes winches/gantries as well as the CMGs. Depending on how the robot is used to compensate path deviations, this central controller might also inform the robot’s controller. Note that other external forces such as wind may also act on the platform and introduce additional errors. The sensors and controllers might be sufficient to deal with such influences, but it could also be that additional sensors and predictive models that modify the target torques of CMGs could further increase system performance.

What remains is the challenge of finding and implementing suitable controllers. For certain parts of the system conventional PID controllers, Kalman filters etc. will be sufficient, while other parts will require more work. The following discusses the underactuation problem and a suitable controller for the dampening task.

5.2 Review of Controller Designs

Control of cranes has been a long-running endeavor, which is obvious from the amount of publications and commercially available systems for gantry and harbor cranes in particular (see e.g. Fig. 52). Given the price and complexity of sensors capable of tracking the position of a hook, these mostly use predetermined actuation paths to reduce the creation of oscillations. This of course is particularly well suited to cranes that perform programmed motions, but can also benefit manual control of cranes.

Figure 52: Sway control in a gantry crane by SWF [70].

Given the flexible connection (ropes) between actuators (winches) and end effector (the hook), lack of sensors and inherent flex of the crane’s structure, they make for interesting control engineering problems. The review by Abdehl-Rahman et al. [67] is a good starting point into the literature. One can find work focussed on improving the control of cranes without a feedback loop such as that by Singhose, Kim and colleagues [71]–[73] who also evaluated the improved performance of operators using such systems [74]. Others have extended input shaping methods to make them more robust towards parameter changes and disturbances [75].

Once the crane and its payload are modeled as a double pendulum, the problems become even more interesting, as it becomes highly underactuated. Underactuation simply means that we have fewer actuators than degrees of freedom (see Fig. 53). This makes such crane models interesting to researchers dealing with underactuated systems. Controllers for other underactuated systems are also relevant for further investigations.

Many different approaches to crane control have been proposed and studied, covering a whole range of methods (e.g. linear, non-linear, sliding mode, fuzzy logic etc.). Another approach to controlling cranes and underactuated systems in general is to use energy-based controller design techniques. These approaches model the energy of the system and changes due to control inputs. By applying various techniques and theorems (i.e. Lyapunov, LaSalle invariance) controllers can be constructed that are proven to be asymptotically stable. The work by Sun et al. applies this approach to the control of a gantry crane in a small lab setup that emulates a double pendulum crane [76]. In this work they show how the derived controller is robust with regard to parameter changes and external disturbances. An earlier paper by Hoang and Lee shows how such controllers perform on a generic underactuated system and compare it to conventional methods (traditional PD, LQR) as well as more complex approaches (sliding mode control, partial feedback linearization) [77]. Their results indicate that the control approach outperforms conventional methods and comes close to the performance of more complex methods. The authors have subsequently also published a comparative study of several controllers for overhead cranes derived using energy-based methods [78].¹

Figure 53: Illustration of the underactuation of a crane. Modeled as a double pendulum of a point mass and distributed mass, adjustable rope length and a suspension point movable in a plane, the crane has eight degrees of freedom. Conventionally there are only three actors to control this. By adding the CMGs the underactuation is drastically reduced.

5.3 Dampening Controller

Having reviewed the existing research, a controller is designed for the basic 2D pendulum model (see Sec. 4.2.2) emulating the energy based controller designs discussed in the previous section. These can be described as PDα\alpha controllers, where the PD part is akin to those parts of a conventional PID controller. The α\alpha indicates the weighted combination of two state variables of the system. In this case these are the two angles of the two pendulum links. With the target of both of these hanging vertically (θ[12]=0\theta_{[12]}=0) this means creating a PD-controller where the error of the two angles is combined:

E=θ2+αθ1\begin{equation} E = \theta_2 + \alpha \theta_1 \end{equation}

The efficacy of this approach can be seen in Fig. 54 and Fig. 55 where an uncontrolled system, proportional control, PD-control and PDα\alpha-control that is informed by the state of both the upper and lower link are compared.

Figure 54: Comparison of various control regimes for a double pendulum with a control torque applied to its lower link. From left to right: a) no control torque b) kP=10,kD=0k_P = 10, k_D = 0 c) kP=1,kD=4k_P = 1, k_D = 4 d) kP=1,kD=4,α=0.5k_P = 1, k_D = 4, \alpha = 0.5

Figure 55: Swing angles (in degrees) of the two links (θ1\theta_1 and θ2\theta_2) of double pendulum with various control regimes applied. See Fig. 54 for animation.

5.4 Inertia Estimation Controller

As the rotational inertia of an object cannot be measured directly, the goal is to use values from the inertial measurement unit (or other sensor tracking rotation). Together with the current speed and position of the gimbals the rotational acceleration and torque experienced by the system can then be obtained. Thereby all parts needed to calculate the rotational inertia can be ascertained:

ω̇τ=I\begin{equation} \dot\omega \tau = I \end{equation}

Sensor data is often noisy, which can lead to unwanted jumps in the controller response. It therefore makes sense to use a state observer for the inertia estimation. These come in various forms, but generally take an estimate of the measurement variance as well as the variance of previous estimations into account.

The above calculation of inertia assumes that the inertia of the payload and platform around the Z-axis are to be measured while the pendulum is hanging still. As soon as the pendulum is not still, adjustments have to be made to the way the inertia is estimated. This is because when the pendulum’s kinetic or potential energy are not zero, the acceleration it experiences also depends on those energies in addition to the torque exerted by the CMGs. Fortunately we already have a model of the system that can provide us with an estimate of the amount of acceleration caused by these energies. This estimate can be utilized by certain types of estimator more complex than the ones suited for the estimation at rest.

The benefits of including an estimator are twofold: Firstly it allows for a unification of the dampening and rotation controller. More importantly it would make the controller robust to changes in the inertia of the platform/payload. This is critical to making such a system useful and usable in the real world, as it creates a flexibility in application. Furthermore it creates a more consistent user experience, as operator inputs result in similar motions as the estimator adjusts the control response. It should be noted that estimator introduce their own complexities and issues.

5.5 Considerations for the 3D Case

The move from a 2D model to a 3D model brings many challenges. While most of these lie outside the scope of this work, there are a few considerations that can already be made at this stage.

One of these is how to move the dampening controller from 2D to 3D. While the principle control approach will most likely hold true for the 3D case, the question arises as to the target and calculation of the error. For the 2D case the target and error are simply θi=0\theta_{i} = 0 and −-θi\theta_{i}, respectively. In the 3D case there is the reference frame/coordinate system at the center of mass of the payload which is also the reference frame for the CMG’s torque. Given the current reference frame as determined by the sensors and a target reference frame (and maybe rotational velocities at that frame), it is necessary to find a measure of the error between the two and how to process them in the controller.

This is covered by what is known as attitude control for aircraft and spacecraft. Given that CMGs are not uncommon in spacecraft, attitude control systems (ACS) often take the limitations of CMGs into account. It will be interesting to see how attitude control techniques can be combined with those for crane control, especially since certain issues in ACS, such as singularities arising from the chosen error indicator, disappear given the kinematic constraints of the crane-CMG system [80]. Furthermore, since the inertia of spacecraft might also vary with time as they e.g. deploy their solar panels, a considerable body of work exists that deals with the estimation of inertia and design of inertia-free controllers.

This paper appears to be a reworked conference proceeding [79]. Both the proceedings and paper are published by predatory publishers. The peer review of the work might therefore be of questionable quality. Nevertheless the work is valid and useful.

6 Experiments

The majority of experiments discussed in this chapter revolve around simulations of the combination of a crane (modeled as a double pendulum) and a CMG array. The simulations reveal the various interactions between the systems and the challenges that arise from them. Prior to this, the first section covers the validation of the 2D and 3D double pendulum models as well as the chosen PDαPD\alpha controller’s performance in dampening the 2D model. The latter three sections of this chapter cover the simulated robot tasks, their integration into the pendulum-CMG simulations as well as the preliminary hardware prototype. Apart from the 3D double pendulum model, the experiments utilize a setup limited to a single axis of pendulum motion (see Fig. 31).

6.1 Pendulum Simulation

6.1.1 2D Model

Some of the interaction that occurs within a double pendulum has already been shown in Sec. 4.2.2, with the lower pendulum causing an overlay of higher frequency oscillations. The Fig. 56 and Fig. 57 show how the rotational inertia of the lower mass changes the interaction of the two parts of the pendulum.

Figure 56: Difference between a double pendulum with two point masses and one where the lower mass is modeled as a distributed mass.

Figure 57: Angles and angular velocity comparison of point mass and distributed mass pendulum.

It is significant that the lower link pushes the upper link considerably more, instead of only hanging below it. This is important, as a rope cannot transmit such a force. This means that the CMG platform should use stiff links instead of ropes to attach to the hook of the crane. This also points to a limitation of the model, as the upper link is modeled as a rod, not as a flexible rope. Therefore, in strong oscillations a difference in behavior between our model and the real world should be expected.

6.1.2 3D Model

The following are simulations of the 3D pendulum aimed at validating them through comparison with the 2D model. By exciting the 3D model in a single plane, the results become comparable.

In fig. 58 it can be seen that the motion remains in the plane of excitation as would be expected. It should be noted that the movement is not identical to that of the 2D model, due to the fact that minuscule numerical errors lead to noticeable changes in a chaotic system such as a double pendulum (see discussion in appendix sec. 10.2.1). This is also most probably the reason that once the plane of excitation is rotated (Fig. 59), the motion breaks out of the plane at some point.

Figure 58: Large excitation of double pendulum modeled with equations of motion derived using projected angles. From left to right: front, side and top view. Note how the path begins to differ strongly when compared to the simulation using the 2D model. This is a prime example of how minor changes can cause large differences in chaotic systems such as a double pendulum.

Figure 59: Large out of plane excitation of the same pendulum. From left to right: front, side and top view. Note how the small inaccuracies cause motion outside of the original plane of excitation. These deviations quickly become chaotic for larger excitations.

In the case of the 3D model with distributed mass, another effect occurs. In Fig. 60 the pendulum was given a 2D excitation but the mass was also given an angular velocity around the axis of suspension. The break from the 2D plane occurs immediately, which is most likely due to the impact of gyroscopic reaction from the interaction of pendulum motion and inertia around the axis of suspension.

Figure 60: Simulation of a distributed mass hanging from a point mass. Initial excitation lies in a plane, but the mass is also given an angular velocity. Note how the pendulum breaks out the of the plane, most likely due to gyroscopic effects.

6.1.3 Dampening Controller

In Sec. 5.3 it was seen that a rudimentary PDα\alpha controller was able to dampen the double pendulum. Now the same controller was applied to a selection of models whose parameters have been set to reflect the given laboratory setup, a small fast deployment crane and a large tower crane. The lower links are modeled as having a load attached to them according to the inertia estimates in Fig. 44. Note that this is a worst case scenario, as it assumes the maximum crane load and also the largest excitation. The largest excitation estimate does not stem from the maximum load, but instead from a jib rotation at maximum speed with the load hanging from the very tip of the crane. In reality, the load capacity at the tip is significantly lower than the maximum load (see Fig. 28).

These models are then each excited by 10°, which roughly fits the various base-rate estimates (see Fig. 30). In Fig. 61 and Fig. 62 it can be seen how the controller works for all cranes, dampening the motion over the course of a few oscillations. This is predominantly due to the fact that the control gains are being scaled with the inertia of the lower link (see Fig. 63). This results in up to 50.000 Nm of torque being applied to dampen the largest crane. While technically possible, it is questionable whether this is economically sensible. It can also be seen that the requirements with regard to torque dynamics and workspace scale with the torque requirements.

Figure 61: Dampening of three different model cranes using the same PDα\alpha controller. From left to right: approximate dimensions of the lab setup, a small fast deployment crane and a large tower crane. The parameters are taken from Fig. 28 and Fig. 44. Controller setting: kP=1.0⋅I2,kD=4.0⋅I2,α=0.5k_P=1.0 \cdot I_2, k_D=4.0 \cdot I_2, \alpha=0.5. Initial excitation is 10°, approximating the determined base rates (see Fig. 30)

Figure 62: Angles and velocities of the cranes during the dampening comparison.

Figure 63: Torque used in dampening of the above cranes.

Were one to use a CMG-array incapable of providing the desired torque the dampening would take longer to stabilize the crane, but importantly it would still work. This is due to the oscillatory nature of the dampening problem. As the crane oscillates back and forth, the torque required to dampen it changes direction. So recalling the nature of the torque workspace of a CMG array, it can be seen that the CMGs will try to provide the desired torque until they reach the edge of the workspace i.e. saturate. As the pendulum begins to swing back, the direction of the target torque reverses and the CMG array has its entire workspace in front of it and can once again provide torque. This is part of the behavior that will be investigated in the following section.

The code used for the above simulations can be found in the appendix in Sec. 10.2.3.

6.2 Pendulum-CMG Interaction

As the 2D model only requires torque production around a single axis for stabilization, a model of this array will be attached to the double pendulum to investigate the interactions between CMGs and the pendulum. The following section therefore briefly introduces the steering law and singularity avoidance for the SPCMG. Subsequently the interaction of said singularity avoidance with the dampening controller is simulated.

6.2.1 Scissored Pair CMGs Steering and Singularity Avoidance

The SPCMG is tasked with producing the torque that is to be applied to the lower link of the double pendulum. Since the control input of the SPCMG is the speed of its gimbal motors, a steering law is required to translate the desired torque into a gimbal speed.

Figure 64: Abstract model of a scissored pair control moment gyroscope.

Looking at an abstract view of the SPCMG (Fig. 64) and given that the design of the mirrored pair dictates that δ=δ1=δ2\delta = \delta_1 = \delta_2 and hr=h1=h2h_r = h_1 = h2, the torque produced by the array can be easily determined:

hnet=2hrsin(δ)τ=hneṫ=2δ̇hrcos(δ)\begin{align} h_{net} &= 2h_r\sin(\delta)\\ \tau &= \dot{h_{net}} = 2\dot{\delta}h_r\cos(\delta) \end{align}

Ergo, the steering law is (WW denoting a target value):

δẆ=τW2hrcos(δ)\begin{equation} \dot{\delta_W} = \frac{\tau_W}{2h_r\cos(\delta)} \end{equation}

From this steering law, it is obvious that −90°<δ<90°-90° < \delta < 90° must be ensured to avoid dividing by zero and hence also avoid the singularity of the array. Since in reality the gimbals have limited acceleration (δ̈max\ddot{\delta}_{max}), δ̇W=0\dot{\delta}_W=0 must be overridden with sufficient breaking distance (δBD\delta_{BD}) to the singularity. In the following YY denotes the current state of the gimbals.

δBD=δ̇Y22δ̈maxδ̇′W={0,ifδmax−(δBD+|δY|)≤0andδY⋅δ̇W≥0δ̇W,otherwise\begin{align} \delta_{BD} =& \frac{\dot{\delta}^2_Y}{2\ddot{\delta}_{max}} \\ \dot{\delta}'_W =& \begin{cases} 0, & \text{if} \quad \delta_{max} - (\delta_{BD} + |\delta_Y|) \leq 0 \quad \text{and} \quad \delta_Y \cdot \dot{\delta}_W \geq 0 \\ \dot{\delta}_W, & \text{otherwise} \end{cases} \end{align}

The conditional tests for two factors:

Does breaking have to be initiated given the current speed and position?

Is the desired gimbal speed causing movement towards the singularity?

The latter condition is important, since there might be a case where the gimbal speed set by the steering law moves away from the singularity and it would not make sense to override this speed with zero.

6.2.2 SPCMGs Steering during Dampening

First the steering law and singularity avoidance must be validated. In Fig. 65 there is a double pendulum approximating to the given lab setup. The difference between the two simulations lies in the velocity of their gyroscopes (1000 rpm and 5000 rpm respectively). It can be seen that the dampening is effective for both cases, but the lower gyroscope speed leads to the CMGs having a smaller torque workspace. So the behavior outlined in the previous section on the dampening controller can be clearly observed here. As the pendulum oscillates, the SPCMG alternates between its two singularities, repeatedly producing torque in between, to dampen the oscillation.

The difference in final gimbal angles (see Fig. 66 and Fig. 67) once the respective models have come to rest is of note. The slow gyroscopes result in an end state closer to the center of the SPCMGs workspace. This is due to the asymmetric torque targets that result from the much faster dampening in the model with the faster gyroscopes. This points to some interesting questions regarding control optimized to position the CMGs in an advantageous position within their workspace.

Figure 65: Comparison of SPCMG singularity avoidance with different gyroscope speeds (1000 rpm and 5000 rpm). The narrow cylinder pointing out of the disc indicates the direction of the angular momentum vector of the gyroscope.

Figure 66: Behavior of the singularity avoidance mechanism for the scissored pair configuration. Note that the speed of the gyroscopes and the maximum acceleration of the gimbals have been set to extremely low values to better illustrate the singularity avoidance.

Figure 67: Singularity avoidance mechanism for the scissored pair configuration at higher gyroscope speed. Note that the maximum acceleration of the gimbals is lower than the maximum attainable with the prototype. This not only helps illustrate the singularity avoidance mechanism but also reduces the axis torque introduced by the gimbaling motion (see discussion in following section).

6.2.3 Dynamics of CMG attached to Pendulum

Recalling the various components of the CMG’s torque discussed in Sec. 3.1, these components (Fig. 68) and totals (Fig. 69 ) can be plotted for the 2D pendulum simulations.

Figure 68: Components of torque produced by a single CMG in a SPCMG array dampening the motion of a double point mass pendulum.

Figure 69: Total torques experienced by the platform and gimbal motor due to a single CMG in a SPCMG (same simulation as Fig. 68).

There are several observations that can be made from the above plots. First, there are a few SPCMG specific behaviors. In this model the desired torque output should be produced around the Z-axis. As the two gyroscopes rotate in opposite directions, the output torque in Z is the sum of the respective output of the two CMGs. The gimbal rotates around the Y-axis, ergo this is where the gimbal motor torque acts. As the CMGs move from their center position towards the singularities, the torque being produced increasingly acts around the X-axis instead of the desired Z-axis. This is why as the singularity is approached, the gimbals speed up to maintain the output around the Z-axis. Inversely, the reaction torque stemming from the base rate of the lower link interacting with the gyroscopes inertia is strongest when the CMGs are in the center position and goes to zero as they approach the singularity. Note how the maximum reaction torque occurs as the pendulum passes through the vertical for the second time, as here the base rate is highest and the CMGs are also roughly in their center position.

In the case of the SPCMG, both the motor torques and reaction torques cancel out, which is why the above figures plot an individual CMG. In other arrays the interactions will be more complex, but similar in principle.

Several observations can be made that are relevant for sizing and arranging these CMGs:

Regarding the gimbal motor sizing, the reaction torque due to the base rate dominates the torque requirements for the gimbal motor.

The torque required to overcome the inertia of the gimbal assemblies might be on the low side. This is firstly due to bearing and gearing friction not being taken into account, but more importantly the dampening control does not require highly dynamic gimbal motions.

As previously noted, the reaction torque due to gimbal rotation in negligible, so it will not be included in the sizing considerations.

6.2.4 Impact of Reaction Torque on the Gimbal Motor

As the reaction torque dominates the gimbal axis, the interaction of the base rate with the dampening motions of the gimbals requires a deeper discussion. The interaction is visualized in Fig. 70 and Fig. 87. The intricacies of this interaction stem from simultaneous oscillations of both the pendulum and gimbal.

When the dampening control causes the zero crossings of the pendulum and the gimbals to align, it creates the maximum reaction torque around the gimbal axis. This is illustrated in Fig. 70. However, the direction of the reaction torque is in line with the desired gimbal rotation, as it produces a torque that counters the pendulum’s motion. The Fig. 87 illustrates how control is maintained over the experienced reaction torque via control of the gimbal position. In the extreme case of the gimbal aligning the gyroscope’s axis with the rotation of the pendulum, the reaction torque will always be zero, regardless of the magnitude of the base rate. The consequences of this interaction are discussed in the sizing section (Sec. 7.2.2).

Figure 70: Interaction of the pendulum motion with the CMGs can lead to strong reaction torques depending on the gimbal angle. The blue arrow is the angular momentum of the gyroscope, it therefore remains constant. The green arrow is the angular velocity that the CMG experiences due to the pendulum’s oscillations, i.e. the base rate. The red arrow is the cross product of these two vectors, i.e. the reaction torque around the gimbal axis. The magnitude of the reaction torque experienced by the gimbal assembly is denoted by its color (green being zero, red being the maximum). The semicircular bar denotes in color the reaction torque that the gimbal axis would experience at the other gimbal angles given the current base rate.

6.2.5 Gimbal Motor Model

The performance of the SPCMG, or any CMG array, is dependent upon the characteristics of the gimbal motors. To ensure realistic torque production, the model limits both velocity and acceleration of the simulated gimbals. Their jerk is not limited.

6.3 Robot Tasks

As described in the modeling chapter, a set of robot paths and both the inverse kinematics as well as a real robot can be used to obtain axis values for the paths. Having run the paths at several speeds, the axis values can be fed into the multi body simulation of the robot to obtain the torques and forces acting at the base of the robot.

Figure 71: Multi-body robot simulation of the four corners task using values from KUKA|prc. Note how the unlimited acceleration in the inverse solution leads to unrealistically high forces and torques.

Figure 72: Multi-body robot simulation of the four corners task using values of a real robot obtained via mxAutomation.

Fig. 71 shows the axis values, base forces and torques as well as the torque dynamics and workspace for a simulation using the axis values from KUKA|prc. As previously noted, the axis values produced by KUKA|prc produce unrealistic values as they assume unlimited acceleration. Moving to the simulations based on the real axis values (Fig. 72), the much smoother axis values are obvious. It should be mentioned here that more realistic robot simulation tools exist.

Figure 73: Comparison of the base torques for the same path performed at two different speeds. At low speeds the longer time spent out of balance requires a larger momentum envelope while higher robot speeds require greater gimbal agility to achieve the momentum dynamics.

Comparing the values of the same path run at different speeds (Fig. 73) several observations can be made. Two types of torque from the robot have to be dealt with: the torque caused by its movement and that caused by its center of mass moving out from underneath the base. The latter lower results in the angular momentum values (Nms) that can be seen for the faster robot, as it spends less time out of balance. On the other hand, the faster robot motion leads to higher torques and more importantly higher torque dynamics. This is one area where it might be possible to optimize robot paths to accommodate the capabilities of the CMGs (see discussion of space robots in Sec. 2.2.1).

Should processes cause the robot, load or kinematic system in general hanging from the platform to be out of balance for a longer period of time, adding a sliding or pivoting weight to the platform could be beneficial. This would not only reduce the workspace requirements of the CMGs, but could also be used to desaturate the CMGs, as it would provide a low agility but infinite source of torque.

Note that the CMGs cannot directly compensate the forces acting at the robot’s base (see Sec. 4.4). Also, the above model and simulations to do not include forces and torques produced by the process itself. So, for example, the force needed to press a drill into a wall is not included.

6.4 Simulated Process Compensation

To ascertain the principle feasibility of stabilizing a robot with CMGs, the robot model is attached to the pendulum-SPCMG model. The base frame of the robot model is attached to the frame in the center of the SPCMG platform so that it moves with the platform. A second copy of the robot model is placed in the same position, but with its base fixed in space. The torques simulated at the base of this latter robot model are fed into the equations of motion of the double pendulum. They therefore act in addition to any torques produced by the CMGs. The same robot torque values are also fed into the control loop for the CMGs as described in the previous section on controller design (Sec. 5). As such a simulation does not include any external forces, it should ideally lead to the CMGs perfectly compensating the torques produced by the robot and lead to a static platform.

Figure 74: Simulation a robot hanging from the SPCMG platform. The left shows how robot deviates from the desired path (transparent robot) as its motion causes the pendulum to swing. The right shows the active compensation by the SPCMGs. Their torque output is informed by a simulated robot model in addition to the pendulum angles. Note how the limited acceleration of the gimbals prohibits the CMGs from perfectly compensating the robot.

Figure 75: Comparison of the robot’s endeffector position while hanging from the pendulum with the programmed target value for simulations with and without compensation by the CMGs.

The Fig. 74 and Fig. 75 illustrate the impact of the CMG stabilization on the robot’s path accuracy. In the animation, the fixed robot is displayed as a transparent overlay. It can be observed that the CMG stabilization significantly improves the accuracy of the robot, but does not succeed in maintaining perfect stability. This is due to the limited gimbal acceleration, which prohibits the CMGs from perfectly following sharp accelerations of the robot arm.

A further challenge to CMG stabilization of the robot’s motion can also be seen in the above simulations. As soon as the robot arm moves its center of mass out from under the center of mass of the pendulum, the CMGs have to constantly rotate to produce the necessary torque to compensate this. Even in the relatively short path simulated here, the CMGs move close to their singularity, where they would no longer be able to produce the torque necessary to maintain the stable position.

These simulations therefore aptly illustrate both the potential of CMG stabilization for robot motion as well as its challenges. Robot stabilization necessitates careful matching of robot trajectories to the gimbal agility and momentum envelope of the CMGs. For instance, in the case above, path optimization could reduce the acceleration spikes in the robot’s movement, thereby improving the path accuracy without requiring stronger gimbal motors. Likewise, by moving the robot arm to the left side between targets the CMGs would be moved away from the singularity, thereby providing greater momentum workspace for subsequent operations.

Limited gimbal acceleration, forces from the robot and other external sources as well as sensor and model inaccuracies will ensure that perfect stability will never be attainable. Additional compensation mechanisms for processes should therefore be considered. Aside from adding thrusters to create compensating forces, the process path accuracy might be able to be improved by using the robot or other kinematic systems to compensate the error. This would take careful control engineering so as not to exacerbate the error by introducing further forces and highly dynamic torques. Given the availability of the crane and robot model, it could be possible to create a predictive controller that compensates the pendulum motion in coordination with the dampening control of the CMGs or even the crane.

6.5 Hardware Experiments

6.5.1 Prototype Setup

Development of the first hardware prototype began early on and ran alongside the remaining project. The goal was to validate the theoretical work rapidly to avoid unprofitable efforts. The size of the prototype was intended to be large enough to handle the torques of the smaller robots of the lab while requiring only a modest hardware budget.

The plan was to begin with a SPCMG array that could be suspended so as to limit the motion to a single plane i.e. 2D motion (see Fig. 31). Later the intention was to extend the array to a four CMG roof array. Given the limited manufacturing facilities, the design uses as many readily available and low cost components as possible.

Figure 76: Hardware SPCMG prototype with attached robot. Gyros are in the aluminum cases with the gyro motors mounted on their sides. The gimbal motors hang underneath the gyros. The motor controller and power supply are mounted underneath the platform. The gyro controller, IMU and communication interface for these are mounted on top. Note that the rope suspension was rotated by 90° for the picture.

Figure 77: CMGs used in the hardware prototype. The design has only two custom parts (gyro wheel, mounting flange) with the rest being catalog components (incl. made to measure shafts).

Fig. 76 shows the completed SPCMG prototype with the KR3 robot hanging underneath. The gyroscopes are housed in the aluminum cases and have their motors attached to the side with risers to accommodate the couplings. The gimbal assembly consists of a single bearing block that holds the axle that is clamped into a block attached to the inside of the gyroscope case (see Fig. 77). This setup leads to only two parts per CMG having to be machined: the gyroscope’s rotor and the mounting bracket for the gimbal.

The gimbal motors are the same as the gyroscope motors, but geared down to provide higher torque. The gimbals require position readings and accurate velocity control. Therefore the gimbal motors have optical encoders attached and are attached to EPOS-70 controllers. The controllers are programmable via a USB interface and are controlled via CAN bus from a PC running Simulink Desktop Realtime using a USB-CAN adapter. See Fig. 78 for the main components used in the prototype.

The gyroscopes require a simpler speed controller as they should not experience any dynamic loads. Therefore DEC-40 development boards were used attached to a custom PCB with an ATTiny microcontroller (see Fig. 79) that provides a serial interface for speed and direction of the gyroscopes. The same PCB and interface provide communication to the BNO055 inertial measurement unit.

Figure 78: Main hardware components used in the prototype.

The CMGs are attached to a welded steel frame that was sized to also later accommodate four CMGs. The controller and power supplies are mounted to a board inside the frame. The frame is suspended from two hooks in the ceiling.

Figure 79: Custom PCB holding the microcontroller, gyro speed controllers, CAN and Ethernet jacks.

For the SPCMG to work as intended, it is necessary to maintain symmetry between the two gyroscopes. This is generally achieved by linking the two gimbals mechanically and using a single actuator to drive them. The use of a mechanical linkage is simple to implement and offers the added benefit of dealing with the reaction torque caused by motion of the base system (see discussion in Sec. 3.1).

Given that it was intended that the prototype should later be extended to a four CMG roof array, it was decided to enforce the SPCMG symmetry with a control loop. The controller applies a proportional gain of the difference in angle between the two gimbals to the desired gimbal velocity. By enforcing the symmetry with a control loop it is also possible to perform experiments regarding zero-torque maneuvers to align the gyroscope axes with the axis of rotation. This is of interest for applications where a manual rotation of the platform should not result in a reaction torque and interference by the CMGs.

6.5.2 Initial Experiments with the Prototype

The Fig. 80 shows some first dampening experiments with the hardware prototype. The top of the video shows the system being excited with the gyroscope axis oriented parallel to the axis of rotation and the control loop deactivated. The lower half of the video shows the system given a similar excitation, but with the PDα\alpha controller active. It can be seen how the motion of the gimbals dampens the initial pendulum motion.

Figure 80: Dampening experiments on hardware prototype.

Yet, also a multitude of problematic issues can be observed. Firstly, the initial pendulum motion is dampened, but an out of plane pendulum motion is created. There are several reasons for this:

points of suspension are too close together to prevent out of plane motion

SPCMGs cannot compensate out of plane motion

the control loop is not precise/agile enough to maintain perfect gyroscope symmetry

The asymmetric motion of the gimbals produces minor torques out of alignment with the in-plane pendulum motion. The inability of the SPCMG or method of suspension to compensate these torques leads to the behavior seen in Fig. 80. The dampening also takes considerable time and once it is completed slightly erratic gimbal motions can be observed.

While these initial results are somewhat disappointing, they show the fundamental validity of the approach. More importantly, they highlight some of the hardware issues that still need to be overcome. These are discussed in the subsequent section.

6.5.3 Hardware Issues and Recommendations

The following is a list of issues encountered as well as recommendations for improving the setup.

The attempt at creating a fast-spinning piece of hardware with little to no machining was for the most part successful. Nevertheless the CMGs are very loud and a recent reassembly showed that some of the gyroscope bearings have suffered and started to stick. This is partially due to the axle having the wrong thread, leading to standard locknuts not fitting. Should one manufacture/order new axles, slight changes could also create more space in the case, which would ease assembly.

Furthermore, during tests two of the gyroscope couplings broke. This was probably due to misalignment of the motor and gyroscope. Having acquired a 3D-printer, it was possible to rework the gyroscope motor mount to improve the alignment process (Fig. 81). The sticky bearings and misaligned axis are also likely reasons for the gyroscopes taking different periods of time to reach their set speed.

The integrated speed readout via the microcontroller has proven very useful. The custom PCB has also increased the reliability of the prototype by introducing high quality plugs. These not only make disassembly much easier, but also reduce the uncertainty from loose/unreliable connections, making debugging easier.

Figure 81: Redesigned mount for the gyroscope. Note how the motor can be removed without affecting the alignment of the mount, making it easier to align the gyroscope’s shaft with the motor. The coupling is also 3D-printed, since the original broke, most likely due to misalignment.

The speed controllers are not able to bring the motors to their maximum velocity expressed by the datasheet. It was also necessary to adjust their velocity measurement by validating the velocities with a tachometer. Both of these issues deserve deeper investigation.

The CAN communication with the gimbals works well, but took a long time to set up. The required commands were extracted from the datasheets and implemented in Simulink. One major issue is the ability to interface with CAN from Simulink. The various CAN adapters supported by Simulink do not have driver support for all modes of Simulink (Desktop Realtime, Realtime) and can be quite expensive or require a dedicated target PC if sensor values are to be monitored simultaneously.

Given these issues and price points of the industrial grade motors, interface cards and software products, it might make sense to look elsewhere for solutions. Since the start of the project, open-source hardware solutions such as the ODrive have emerged. The ODrive can combine lower cost hobby brushless motors, that have become very powerful due to drones, e-scooters etc., with encoders to create much more affordable servos. Together with belt drives or cycloidal gears, backlash free drives could be produced using mostly 3d-printed and off-the-shelf low-cost parts. Generally speaking, the prototype could probably be replicated at lower cost if greater use were made of printed parts and consumer grade hardware.

Alternatives to Simulink and specialized target PCs for projects such as this one are also emerging. The microcontroller on our custom PCB is probably sufficient to create a UDP to CAN bridge. As such our reworked PCB also accommodates ethernet as well as CAN hardware. The interested reader is pointed towards:

Modelica, xcos and X2C

OpenRTDynamics

GRiSP and Nerves

7 Sizing CMGs for Cranes

The models, simulations and experiments of the previous chapters provide a basis for understanding the behavior of the crane-CMG-robot/process system. This chapter attempts to summarize the insights gained regarding the sizing of the CMGs for such a system. Its first section covers sizing considerations for the three application categories set out earlier (dampening, rotation, process compensation). The section first discusses the relationships between the requirements and system parameters before covering the sizing considerations specific to the crane-CMG system.

7.1 Requirements

Recalling the properties of a CMG array, there are three main requirements:

maximum torque produced (Nm)

dynamics of the torque (Nm/s)

torque capacity i.e. momentum envelope (Nms)

The created envelope does not have to be symmetric and the dynamic performance is not homogenous within the envelope. The steering law for the array will also change the torque availability depending on the chosen singularity avoidance techniques.

7.1.1 Dampening

The performance requirements for dampening depend on the specific need for dampening. Generally, however, the sizing will be informed by:

amount of excitation to dampen

crane and load parameters

max duration/swings until dampened

The section on modeling cranes and loads (sec. 4.1) covers a model to estimate the relevant parameters, including the amount of excitation to be expected. The chaotic interaction inherent to a double pendulum makes it difficult to create a way of determining/estimating the time it will take a given CMG array to dampen an excitation. Looking at the dampening simulations of different capacity CMGs (fig. 65), the basic behavior during dampening can nevertheless be understood.

The dampening controller will generate torque until the CMGs saturates i.e. its capacity (Nms) is consumed in one direction. As the pendulum reverses direction, the CMGs move out of saturation as the commanded torque also reverses direction.

This indicates a potential to estimate the number of oscillations a CMG array requires to dampen a given pendulum. The estimation will probably relate the momentum envelope of the array to the angular momentum of the two links. This momentum in turn depends on the moment of inertia and maximum rotational velocity of the two links. The developed models nevertheless permit the estimation of the dampening performance by varying the momentum envelope and observing dampening performance (Fig. 82 and Fig. 83, for code see sec:2d-dp-wcontroller-limit) and thereby derive the necessary requirements for the subsequent sizing of the CMG array. These simulations also provide a useful rule of thumb for the dampening performance: doubling the momentum envelope halves the number of swings it takes to dampen the crane.

Figure 82: Simulation of dampening with differently sized momentum envelopes i.e. CMG workspace sizes.

Figure 83: Lower link angle, torque used and position in momentum envelope for dampening with different sized momentum envelopes i.e. CMG workspace sizes.

Depending on the cause of excitation, it might be necessary to further increase the momentum envelope. In the discussed dampening simulations, it can be seen that dampening can leave the CMG array in a state away from the center of its momentum envelope. If the process-compensations require a certain volume of momentum, the envelope could be increased to ensure sufficient volume remains after dampening. Alternatively, as discussed in the controller design section, it should be possible to develop dampening control that extends the dampening duration but leaves the CMG array closer to the center of its momentum envelope.

7.1.2 Part Rotation

Rotating a load around the axis of the lower link is similar to the reorientation of a spacecraft using momentum control devices. Therefore spacecraft literature provides useful approaches.

For satellites neither part rotation nor the speed of rotation are usually the main concern. Instead most interest is in determining how long it will take to rotate to the desired position. This of course depends on the rotation speed, but also on the maximum acceleration and jerk (derivative of acceleration). This is particularly true for short rotations where the maximum speed might not be reached before decelerating. See [26, Sec. 3.1] for an in-depth discussion of this and how the duration of the jerk and acceleration-limited regime can be derived from the relationship between maximum jerk, acceleration and velocity.

Whilst the optimization of these relationships is of great relevance to spacecraft design, for the part rotation on cranes it can be assumed that rotation performance is mostly concerned with large rotations. Therefore, the sizing requirements can be simplified to:

ω=hI\begin{equation} \omega = \frac{h}{I} \end{equation}

where ω\omega is the desired rotation speed of the platform/load/part, II its moment of inertia about the axis in line with the lower link and hh the combined angular momentum of the gyroscopes in the CMG array.

7.1.3 Process Compensation

Process compensation brings with it the most complex and demanding requirements. As discussed in the simulations of the robot paths, it is necessary to look at the peak torque, torque dynamics and total torque consumption. By optimizing the robot paths or trading dynamics for consumption, the design of the process can change the requirements significantly. It will therefore be beneficial to develop tools that provide a short feedback loop for the process designers.

This work provides an example of such a tool with the ability to evaluate the requirements of a parametric robot path made with KUKA|prc. Currently this is still a multi-step process that could certainly be more tightly integrated. The simulation of the obtainable path accuracy through the use of process compensation currently does not take into account the ability of the robot to adapt to deviations of its base. Nevertheless an estimation of the path accuracy without such additional compensation techniques has been shown in Sec. 6.4 and the existing tools can already provide the fundamental requirements of Nm, Nm/s and Nms for the process compensation. The interaction with additional robot-based compensation might lead to an increase in the torque dynamics requirements, but the torque and envelope requirements should remain similar.

7.2 Sizing of CMGs

7.2.1 Parameter Relationships

The various requirements discussed above are illustrated in Fig. 84. Having determined the requirements the next step is to understand the relationships between the various parameters of the CMG array. These are illustrated in Fig. 85.

Figure 84: Illustration of the requirements stemming from the different CMG tasks.

Figure 85: Illustration of the relationship between the parameters of a CMG array and crane.

Beginning with the gyroscopes, there is the moment of inertia of the rotor. This can be affected by the density of the material as well as by the shape and size of the rotor. The simplest forms are cylinders with cylindrical shells providing better mass utilization. Another common design are spherical rotors which have the added benefit of causing less changes to the moment of inertia in the array/platform/spacecraft when rotated about their gimbal axis. At high velocities rotors can experience significant forces, and this needs to be taken into consideration when selecting the material and sizing the rotor.

The moment of inertia and rotor speed provide the momentum for each gyroscope in the array. A rudimentary approximation of the size of the momentum envelope can be obtained by summing the moments of all gyroscopes of the array. The actual envelope shape depends on the chosen array type and parameters. As discussed in the section on CMG envelopes, asymmetric envelopes can be utilized to match asymmetric torque requirements. An array tasked predominantly with large-part rotation could be designed to have its largest extent in the Z-axis, whereas an array tasked with dampening would prioritize the other axes.

The size of the gyroscope motor is governed by the required velocity, desired spin up time and amount of friction in the system. If slower spin up is acceptable and the gyroscope exhibits little friction (large gyroscopes often run inside a vacuum to reduce drag), the gyroscope’s motor can be very modest.

The moment of inertia of the rotor around the gimbal axis can also be used as an approximation for the sizing of the gimbal motors. Whilst the gimbal motor also has to overcome the moment of inertia of the housing etc., the gyroscope’s rotor will be the major contributor to the gimbal’s inertia. As seen in the simulations of the SPCMG, during dampening the sizing of the gimbal motor’s torque is dominated by two factors:

required torque agility

reaction torque caused by the base rate

The required torque agility τ̇B\dot\tau_{B} depends on the momentum of CMG array, the inertia of the gimbal assembly and the torque of the gimbal drive:

τ̇B≈hArray⋅IGimbalτGimbal\begin{equation} \dot\tau_B \approx h_{Array} \cdot \frac{I_{Gimbal}}{\tau_{Gimbal}} \end{equation}

The reaction torque depends on the momentum of the gyroscope and base rate i.e. the angular velocity experienced by the platform. Since the CMG arrays should operate in three dimensions and under diverse loads, the worst case must be assumed i.e. that the momentum vector of the gyroscope will lie orthogonal to the angular velocity. Both the desired output torque produced by the CMG as well as the reaction torque depend on the momentum of the gyroscope. But while the output torque depends on the gimbal velocity the reaction torque depends on the base rate Therefore, the following relationship holds true [26]:

ωgimbalωsystem=τsystemτgimbal(2) \frac{\omega_{gimbal}}{\omega_{system}} = \frac{\tau_{system}}{\tau_{gimbal}} \qquad(2)

In spacecraft this relationship is highly critical, as the torque of a motor is strongly linked to its weight. As previously noted, for crane-CMG systems weight will be a lesser concern, certainly much less so than with space applications. Furthermore, unlike space applications, in our case the base rate is (except for part rotation) not the desired result of the CMGs actions. Instead the dominant base rate originates from the oscillatory nature of the crane as a pendulum. The relevance of this difference will be discussed in the following section.

To make sizing easier, all equations involved can be entered in a spreadsheet (see Fig. 86). Using this method, various parameters can be run through quite quickly.

Figure 86: Spreadsheet of the most relevant CMG parameters including the equations that link them together.

The spreadsheet makes a distinction between torque dynamics at rest and those during the worst case. In such a case the momentum of the gyroscopes would be aligned with the axis of rotation of the array/base and the controller would be trying to move the gimbal in the opposite direction of the precession caused by the gyroscopic reaction. This would lead to the gimbal motor having to overcome the entire reaction torque prior to being able to produce any of the desired torque. To illustrate this, the spreadsheet subtracts the reaction torque from the maximum torque of the gimbal motor. The following section will discuss why this scenario overestimates the worst case reaction torque and how it me be further reduced.

To provide a better intuition of the characteristics of the CMGs, the spreadsheet also calculates the angle of rotation required by the gimbal motor to reach its maximum speed from a standstill. In an SPCMG array a singularity will be encountered after rotation of the gimbal through 90° when starting from a neutral position, therefore this value gets highlighted yellow once it goes above 90°. In other arrays the singularities will be different, but it will probably be difficult to achieve the maximum gimbal velocity if more than 90° are required to reach it.

The spreadsheet does not take friction or the exact shape of the momentum envelope into account. The actual inertia of a gimbal assembly will also be higher than the inertia of just the gyroscope’s rotor. All of this can most likely be taken into account with scaling factors, once some prototypes are providing data points.

7.2.2 Sizing CMGs for Cranes

The challenge of sizing CMGs for cranes is the unique combination of requirements arising from the three application cases of damping, rotation and process compensation. Whilst commercial applications and sizing experience exist for spacecraft control (similar to part rotation) and roll stabilization (similar to dampening), the addition of process compensation as well as the need to operate around three axes creates novel challenges. The core challenge is the high base rate that our CMGs experience when they are attached to a swinging crane.

In a spacecraft the base rate is dictated by the agility requirements, which in turn leads to requirements regarding the output torque of the CMGs. The relationship of torques and velocities in a CMG (see Eq. 2) the sizing of the gimbal and gyroscope motor. Unlike the case of crane dampening, the reaction torque here is self-inflicted. Also, since most maneuvers will produce a net change of zero in the CMG array’s momentum, the size requirements regarding the envelope are much smaller than in these cases, where asymmetric external influences have to be compensated.

While the base rate in ship stabilization is more similar to our base rate, it is not possible to imitate their sizing approaches. In ship stabilization the base rate i.e. the roll is in fact utilized, as the precession causes exactly the gimbal movement necessary to produce torque counteracting the roll of the ship. Gimbal actuation is only used to assist the gyroscope in overcoming friction, to limit the response and avoid hitting the singularities. In this work, however, the same CMGs have to be used to provide torque compensation while simultaneously dampening oscillations.

Given the requirements for dampening and process compensation previously discussed, the size of the momentum envelope is paramount to the usefulness of the CMGs for crane applications. Obviously, the size of the envelope is governed by the momentum of the gyroscopes. Therefore, given a certain base rate, reaction torques will always have to be dealt with that outweigh the gimbal torque requirements resulting from torque dynamics requirements (see discussion in Sec. 6.2.3 and Sec. 6.2.4).

To make CMGs feasible for an application in cranes, ways to alleviate the base rate or to be more precise: its impact on the gimbal motor sizing must be found. This is the key issue facing the crane-CMG-robot system and the following solutions come to mind:

limit CMG operations to certain wind loads and crane movement speeds

introduce a clutch mechanism to the gimbals

align the gyroscope’s momentum with the axis of the base rate

As can be seen in the simulations of the SPCMG during dampening (Fig. 65), the momentum vector of the gyroscopes passes through a configuration parallel to the axis of rotation of the platform as it is dampened. It should therefore be possible to add a switching behavior or otherwise modify the controller to ensure a certain amount of alignment in relation to the current base rate and its acceleration. It is hoped that the animation in Fig. 87, helps to illustrate the challenge. In the current configuration, the gimbal experiences zero reaction torque through the entire swing of the pendulum. However, as the gimbal is not moving, there is also no torque produced to dampen the oscillation. Assuming that yellow denotes a reaction torque harmful to the gimbal assembly, the challenge is to rotate the gimbal past the midpoint prior to it turning yellow.

Figure 87: Illustration of the potential reaction torque acting on the gimbal, depending on the current base rate and gimbal angle. Note how, as the gimbal axes are held parallel to the axis of rotation, no reaction torque is produced. The blue arrow is the angular momentum of the gyroscope, it therefore remains constant. The green arrow is the angular velocity that the CMG experiences due to the pendulum’s oscillations, i.e. the base rate. The semicircular bar denotes in color the reaction torque that the gimbal axis would experience at the various gimbal angles.

It is also important to note that the direction of reaction torque is in line with the desired rotation of the gimbal. Under a well-designed control regime, the maximum allowable reaction torque might lie above the maximum torque rating of the motor and other parts of the gimbal assembly. As the motor and reaction torque are pushing in the same direction during the dampening process, the limits might actually lie within the current limits and electronics design of the controllers and not the torque limit of the motor.

All of this could lead to a stark reduction in the impact of the reaction torque as compared to the worst case scenario assumed in the sizing spreadsheet. Nevertheless, it is recommended that dedicated points of failure for gimbal motor couplings be added and perhaps even means to mechanically arrest the gimbals’ motion should a failure of the coupling occur.

8 Outlook and Conclusion

Prior to summing up this work, I would like to discuss the assumptions and associated limits of the models currently in use. Given these limits and some of the insights gained through the experiments, I will then make recommendations regarding the most urgent developments needed to further the goals of this work. I will also cover further questions and areas of research that need to be addressed to make CMGs a useful tool for cranes in general and the construction site in particular.

8.1 Assumptions of Current Models

The most significant simplifications in the models are, (not in order of importance) :

use of stiff links capable of transmitting torque
(possible for lower link, unrealistic for upper link)

no drag or friction

no process forces or torque in robot simulations

no sensor noise or control loop delay

The sizing calculations also do not include any considerations concerning the specific shape of the momentum envelope, its singularities and changing dynamics as one moves through it. Most of these assumptions do not limit the conclusion made from the simulations, but they do limit further understanding of the system and in particular the development of controllers.

8.2 Next Steps for Development

With the goal of moving towards a 3 DoF CMG system suited for real world crane stabilization experiments, several parts of this work need to be transferred to 3D cases. Nevertheless the 2D models and prototype should be of continued use. Both controller and hardware design as well as validation can more easily be understood and performed on the 2D case.

A priority for the development of the 3D controller should be the inclusion of the reaction torque limiting strategy discussed at the end of the previous chapter on sizing (sec. 7.2.2). Limiting the reaction torque experienced by the CMGs through alignment of momentum and rotation axis is the critical component required to make CMGs useful for crane-based applications beyond load rotation. The development described in this section therefore mostly outlines the critical path to a state where the efficacy of such a strategy can be validated through simulations.

It is to be hoped that the existing explicit equations of motion together with the SymPy code used for their derivation will prove useful in the design and stability proofs of the required controllers. To be able to develop and also validate the steering and control of the four-CMG roof array (required for the move to 3DoF hardware prototype), either a new modular model or the existing 3D equations of motion could be used. The existing model will already allow developments regarding the dampening and rotation applications. One thing currently not included in the 3D equations is the translation of external forces into additional torques on the upper and lower link. This has so far only been implemented for the 2D model.

To move the controllers (and subsequently the hardware) to 3D, one will have to translate the error about three axes into a 3D target torque. This in turn necessitates the extension of the SPCMG model to a four-CMG roof array with the implementation of a suitable steering law. Once these components are in place, it will become possible to analyze the controller’s performance under the limitations resulting from the agility and singularities of the 3D CMG array in combination with the chosen steering law.

The biggest challenge in limiting the reaction torque through control of the gimbal angles will be that it will probably require a novel steering law for the CMG array. While many steering laws and steering law design strategies exist, (see [26, Ch. 7]), none deal with this very crane-specific constraint.

Apart from the reduction of reaction torques, the other major question is how well a CMG array is able to provide a stable platform for robotic and other processes. While torques acting upon the platform itself can be compensated by the CMGs, forces acting on it end up acting on its pivot point and therefore on the link from which the platform is hanging. This can only be compensated by the CMGs indirectly i.e. by tilting the platform. This is not taken into account in the controller currently implemented. A controller that reduces the error produced by the force acting on the upper link by introducing an error to the lower link might improve platform stability.

Furthermore the forces and torques applied to the pendulum during the simulated compensation assumed a fixed robot. The motion of the robot’s base due to oscillations of the platform are therefore not taken into account in the forces and torques. To address this issue, the robot multibody simulation needs to be integrated into the pendulum model. Through the addition of either more realistic inverse kinematics that take axis accelerations into account, or a hardware-in-the-loop setup that uses the actual robot controller, robot-based compensation strategies could be developed, enabling these strategies to be validated as they work in coordination with the dampening controller.

Given that it will also be desirable at a later date to extend the model to include the remainder of the crane, my recommendation would be to implement a modular multibody simulation into which the model of the CMG dynamics can be integrated. This should be possible in Modelica, Simulink Simmechanics or other toolkits. While the CMG array could also be modeled using this approach, I would recommend the use of the explicit model used in this work, as it makes it possible to distinguish the various torque components. This proved to be of great value in understanding the gyroscopic system.

8.3 Future Work Required

Beyond the critical next steps outlined above, there is a plethora of research and engineering challenges.

Once the pendulum, CMG and robot multibody models as well as the inverse kinematics have become more tightly integrated, optimization strategies to increase process accuracy through changes to robot paths and execution speeds/accelerations could then be studied. Similarly, by extending the models to include motion of the suspension point i.e. gantry motion or trolley and gib motion, control systems become possible that utilize information and actors of both the crane and CMG array. This should open up novel control techniques for the controlled movement of cranes as well as better input to the reaction torque reduction and ways to desaturate the CMGs. Desaturation goals could also be met by adjusting control during the dampening of large oscillations. Furthermore the use of an adjustable weight as a source of low agility torque would require further extensions to the CMG steering laws.

Such adjustable weights would be added to the list of engineering challenges involved in bringing CMGs to the construction site and other cranes. Prior to achieving on-site suitability, there is also some work to be done in extending the lab prototype from one to three degrees of freedom. The gyroscopes should be improved with regard to their rotational accuracy and mounting so as to reduce wear, spin up times, noise and vibrations. The issues with the maximum gyroscope speed and accuracy also need to be investigated. To ease and accelerate the controller design process, the realtime capabilities need to be increased to allow for signal analysis during experiments and rapid modification of parameters without recompiling. By moving the control loops to an embedded target, the round trip times from controller to motors and back should also become shorter and, most importantly, more reliable. Given the risk of high reaction torques, stronger gimbal motors and couplings or dedicated points of failure should be investigated.

The current sole IMU should also be extended to allow for measurement of the upper link. It might also be useful to add external optical tracking to allow for validation of the onboard sensors. It is to be expected that the implementation of the inertia estimation should be straightforward, but it is also a required development.

Looking towards more realistic implementations of the CMG array, the power use needs to be taken into account. While it is possible to provide power at the crane hook, given the advances in battery technology, models and experiments regarding the power consumption of the array during operation will be of interest. As previously noted, gyroscopes in CMGs have been used as additional energy storage to handle load spikes in marine applications. The use of such storage in spacecraft was abandoned due to batteries having a similar energy density while posing less engineering challenges.

Continuing and ever more extensive validation will also be required. The collection of real crane motion data would be highly valuable for the validation of the crane parameter estimations and dampening requirements. As the simulation tools are improved, more realistic and diverse processes will emerge that utilize the crane-CMG system and provide valuable input regarding the compensation and rotation requirements.

8.4 Conclusion

This work was motivated by the desire to bring more robotic capabilities to the construction site. Given the limited reach of conventional robot arms, the idea was to combine them with the reach of construction cranes to be able to service the large workspace of construction sites. To ensure a stable work platform for the robot arm, the strategy was to begin with a parallel tendon kinematic, iteratively remove tendons and add other form of compensation mechanisms to maintain stability for the robot. My initial research into potential compensation mechanisms led to spacecraft momentum control devices and the large torque capacities of control moment gyroscopes. The focus of this work therefore became to ascertain the potential of CMG stabilization of crane hooks.

The review of the general principles of CMGs, their use in spacecraft, ships and other applications including cranes shows the knowledge gap that this work hopes to begin to fill. There is a large amount of literature regarding the design and control of CMGs for spacecraft attitude control. The same is true of crane dynamics and control theory. Up to now there have only been a handful of research efforts regarding the use of CMGs in cranes, none of which provide models of crane-CMG dynamics or guidelines for the sizing and design of such systems.

This work therefore provides suitable models of the crane and platform/load as a double pendulum with a distributed mass hanging from a point mass that in turn is hanging from a fixed pivot. Given the relevance of the base rate on the sizing of CMGs, the work also includes an estimation of the base rate for cranes that uses parameters from crane data sheets. This in particular will require validation, as the base rates will change depending on the control of the crane and excitation sources present during operation.

To understand the interaction of the crane-CMG system, I chose to limit the crane to a 2D model but left the CMG model 3D. This is necessary, due to the cross product involved in gyroscopic systems (eq. 1). By implementing the equations that describe the dynamics of a scissored pair CMG and attaching it to the obtained equations of motion for the crane/pendulum, I was able to develop an understanding of the system much more quickly than with a physical prototype. In the physical prototype, our view into the system is limited by the sensors, noise, signal delay and of course bugs. The simulation models also include the implementation of a steering law for the SPCMG with singularity avoidance and a model of the gimbal dynamics that limit acceleration and velocity of the gimbal.

The simulations and subsequent development of control as well as sizing approaches were systemized through the definition of three-crane-CMG applications: dampening, part rotation and process compensation. This definition helps to distinguish the requirements and interactions of the system. While the part rotation is akin to CMG applications in spacecraft, dampening is more related to the roll reduction systems in boats. The addition of process compensation means that while we can borrow from certain parts of spacecraft design and particularly from the engineering of ship gyroscopes, novel control and sizing approaches are required to fully utilize the CMG’s potential for cranes. The combination of applications also sets apart this work from prior crane stabilization efforts, which only focus on stabilization and limit themselves to a single CMG.

The chapter on controller design develops an integrated control approach for the combination of the three applications. From a review of existing crane dampening controllers, and control of underactuated systems, I chose to implement a simple PDα\alpha controller. Simulations and preliminary experiments on the physical prototype show that this approach is capable of dampening the double pendulum using torque produced by the CMGs.

The process compensation utilizes the fact that the process is known in advance and controlled in a programmed manner. By simulating the torques produced by the process in advance, it should therefore become possible to compensate these torques which would otherwise excite the pendulum. The rotation controller arises naturally from the dampening controller, once the system is extended to three dimensions. All of these controllers require an understanding of the moment of inertia of the platform and load. This might be estimated using conventional techniques (e.g. a Kalman filter) that utilize the understanding of the torque being produced and the observed accelerations of the system.

The simulations created the understanding required to develop the sizing of CMGs for cranes. The interaction of the applications’ requirements and various parameters of CMGs are discussed in the chapter on sizing. The chapter provides a workflow through the sizing parameters which was also translated into a spreadsheet that should assist in sizing CMGs for crane applications.

Together with the simulations, the sizing workflow validates the feasibility of the CMG-crane system. More importantly, the work has also led to the identification of key challenges for the development of CMG-crane systems.

We can see that CMGs capable of producing the torque with an agility suited for process compensation and part rotation are feasible. The issue of the gimbals having to sustain gyroscopic reaction torques caused by rotation of the system is sufficiently limited or controllable in these cases. Processes compensation ideally results in a static platform, and ergo low base rates. During part rotation the base rate is governed by the desired rotation speed. During dampening, however, the base rate is governed by the crane’s parameters and easily reaches velocities similar to those of our gimbals in the other applications. Given the relationship of torques and velocities in a CMG system, this leads to high reaction torques, which in turn cause high gimbal motor torque requirements. This is exacerbated by the fact that dampening performance is highly dependent on the size of the momentum envelope, which in turn necessitates high angular momentum in the gyroscopes which in turn increases reaction torques.

Therefore, the challenge of reducing the reaction torque during dampening is the key to the further development of this work. An in-depth explanation is given in the simulation chapter (Sec. 6.2.4) as well as the subsequent chapter on sizing (Sec. 7.2.2). From the simulations it seems possible to extend the CMG control and steering laws so as to limit the reaction torques by forcing the momentum vectors into alignment with the axis of rotation during the periods of high velocities. The previous sections on future work lay out how the existing models and hardware should be extended so as to allow the development of such CMG control.

The recent arrival of a commercial CMG based system for crane load rotation proves the potential impact of CMGs on crane operation. The development of more advanced, crane specific CMG control for 3 DoF CMG-arrays should extend their utility beyond rotational control during critical lift operations. Their dampening capabilities could lead to high speed, stable crane movements and also allow entirely novel crane-application by providing a stable process platform.

Controlled motion is the foundation for automation. With the addition of CMGs, cranes could become a motion provider for workspaces of unprecedented size and payload. It is my hope that the explanations, models, code, tools and discussions of this work will prove a useful for others pursuing this potential.

9 Glossary

CMG

Control Moment Gyroscope. A momentum control device consisting of a gyroscope mounted on a single gimbal (SGCMG) or dual nested gimbals (DGCMG). The distinguishing differences from the other common momentum control device, the reaction wheel, is that they utilize the effect of gyroscopic reaction torque for produce their output torque instead of relying on the gyroscopes motor. See Sec. 2.1 more information.

SPCMG

Scissored Pair Control Moment Gyroscope. A specific arrangement of two CMGs that results in their combined output torque being aligned along a constant axis. This simplifies their steering laws when compared to other array types. Their downside is that it requires three SPCMGs and hence six CMGs to create torque around all three spatial axes. See Sec. 6.2.1 for more.

Singularity

In the context of mechanisms a singularity is a certain configuration or state of the moving parts where the control or modelling of the system breaks down to a certain degree. This can mean that the behavior after reaching the singularity is no longer predictable or that certain values become infinite or nondeterministic. An example of this for in an industrial robot can be seen in Fig. 88.

Figure 88: Illustration of a singularity in a six-axis industrial robot. Once the two axis are in line they can in theory rotate an infinite amount with producing a lateral movement. The speed at which they have to rotate to maintain a constant lateral motion therefore also goes to infinity. Animation by Mecademic 81

Null Space Motion

The term stems from the concept of state space describing all possible states of a system. Null space is then the subspace in which changes of the state don’t affect the output of the system. In robotics this means the end-effector remains motionless even though the robot axes are moving. In CMG arrays this means the output torque isn’t affected by the motions of the CMGs. Another way of describing it, is that given a desired target e.g. torque, position, path there exist multiple solutions. This redundancy can be used to optimize motions of the system while still producing exactly the desired output.

Error Torque

In the context of momentum control devices any deviation of the output torque from the target i.e. desired torque.

Jacobian

In the context of engineering the Jacobian matrix is the matrix of partial differential equations that describes the relationship between changes in a systems state and the systems output i.e. with 𝐉\boldsymbol{J} as the Jacobian, 𝐱\boldsymbol{x} as the output vector and 𝐪\boldsymbol{q} as the state vector:

d𝐱=𝐉⋅d𝐪 d\boldsymbol{x} = \boldsymbol{J} \cdot d\boldsymbol{q}

Pseudo-Inverse

A generalization of the inverse of a matrix that accommodates non-invertable matrices. The most common example of this is known as the Moore-Penrose inverse. See [26, Ch. 7] for a discussion of how alternative pseudo-inverses are designed to optimize CMG steering laws in different ways.

Center of Percussion

The point on a pendulum where a perpendicular force leads to zero reaction force at its pivot point, due to the angular and translational acceleration cancelling each other out.

Figure 89: Illustration of the center of percussion and how it relates to the reaction of a pendulum given the location of a force acting upon it. CC-BY-SA 4.0, Wikipedia user Qwerty123uiop

10 Appendix

10.1 CMG Variables

Fig. 90 lists the properties of the SPCMG used in this work.

Figure 90: Properties of the SPCMG with associated symbols and units.

The angular momentum of the gyroscope can be determined as follows:

h=Iω=πρd2(r24−r14)*ω\begin{align} \text{h} &= I\omega \\ &= \frac{\pi\rho\text{d}}{2} (r_2^4 - r_1^4) * \omega \end{align}

Some useful conversions to SI-Units:

rpm⋅π30=rad/sg/cm3⋅1000=kg/s\begin{align} \text{rpm} \cdot \frac{\pi}{30} &= \text{rad}/s \\ \text{g}/\text{cm}^3 \cdot 1000 &= \text{kg}/\text{s} \end{align}

10.2 Equations of Motion and Simulation using Python

10.2.1 2D Point Mass Double Pendulum

Prior to obtaining the equations of motion for more complex cases, I obtained those of a point mass double pendulum. The following code uses the SymPy Python library which allows us to perform symbolic calculations. The code also validates our results against known equations of motion.

""" EoM for 2d point mass double pendulum """
from sympy import symbols, Function, Eq
from sympy import sin, cos
from sympy import diff, simplify, expand, nonlinsolve
from sympy import init_printing, pprint, latex

init_printing()

""" Define Symbols """
l1, l2, m1, m2 = symbols("l1 l2 m1 m2")
t = symbols("t")
g = symbols("g")
L, Pot, Kin = symbols("L Pot Kin")
testPot, testKin = symbols("testPot testKin")
theta1 = Function("theta1")(t)
theta2 = Function("theta2")(t)
z1, z2, zdot1, zdot2 = symbols("z1 z2 zdot1 zdot2")
x1, y1, x2, y2 = symbols("x1 y1 x2 y2")
q = [theta1, theta2]
substitutions = [
    (theta1.diff(t).diff(t), zdot1),
    (theta2.diff(t).diff(t), zdot2),
    (theta1.diff(t), z1),
    (theta2.diff(t), z2),
]

""" Define Kinematic Relations """
x1 = l1 * sin(theta1)
y1 = -l1 * cos(theta1)
x2 = l1 * sin(theta1) + l2 * sin(theta2)
y2 = -l1 * cos(theta1) - l2 * cos(theta2)

""" Langrangian """
Pot = m1 * g * y1 + m2 * g * y2
Kin = 1 / 2 * m1 * (x1.diff(t) ** 2 + y1.diff(t) ** 2) + 1 / 2 * m2 * (
    x2.diff(t) ** 2 + y2.diff(t) ** 2
)
L = Kin - Pot

""" Validate against known Lagrangian """
testPot = (m1 + m2) * l1 * -g * cos(theta1) - m2 * l2 * g * cos(theta2)
testKin = 0.5 * m1 * l1 * l1 * z1 * z1 + 0.5 * m2 * (
    l1 * l1 * z1 * z1 + l2 * l2 * z2 * z2 + 2 * l1 * l2 * z1 * z2 * cos(theta1 - theta2)
)
if (testPot - Pot).simplify() != 0:
    print("Validation of Potential Energy against known function failed.")
    quit()
if (testKin - Kin.subs(substitutions)).simplify() != 0:
    print("Validation of Kinetic Energy against known function failed.")
    quit()

euleq = [Eq(0, L.diff(qi.diff(t)).diff(t) - L.diff(qi)).simplify() for qi in q]
euleq = [euleqi.subs(substitutions) for euleqi in euleq]

solvedeuleq = nonlinsolve(euleq, [zdot1, zdot2])

for solution in solvedeuleq:
    for eqn in solution:
        print(eqn.simplify())

Subsequently we can copy the resulting equations into a second program to simulate the pendulum. Note how even though we are using symbolically identical equations, the simulation results begin to differ after a while. This is a lovely example of numerical inaccuracy in computers and also how minimal changes in a chaotic system like a double pendulum can cause increasingly large differences in behavior.

"""Simulate Pointmass Double-Pendulum Motion

Original code by Christian Hill, taken from his website scipython.com.
"""
import sys
import numpy as np
from math import sin, cos
from scipy.integrate import odeint
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from matplotlib.patches import Circle

# Pendulum rod lengths (m), bob masses (kg).
l1, l2 = 1, 1
m1, m2 = 1, 1
# The gravitational acceleration (m.s-2).
g = 9.81
# Maximum time, time point spacings and the time grid (all in s).
tmax, dt = 30, 0.01
t = np.arange(0, tmax + dt, dt)
# Initial conditions: theta1, dtheta1/dt, theta2, dtheta2/dt.
y0 = np.array([3 * np.pi / 7, 0, 3 * np.pi / 4, 0])
y0_small = np.array([np.pi / 8, 0, np.pi / 8, 0])

# Plot settings
r = 0.05  # bob circle radius
fps = 25
di = int(1 / fps / dt)

def deriv(y, t, l1, l2, m1, m2):
    """Return the first derivatives of y = theta1, z1, theta2, z2."""
    theta1, z1, theta2, z2 = y

    c, s = cos(theta1 - theta2), sin(theta1 - theta2)

    theta1dot = z1
    z1dot = (
        (
            m2 * g * sin(theta2) * c
            - m2 * s * (l1 * z1 ** 2 * c + l2 * z2 ** 2)
            - (m1 + m2) * g * sin(theta1)
        )
        / l1
        / (m1 + m2 * s ** 2)
    )
    theta2dot = z2
    z2dot = (
        (
            (m1 + m2) * (l1 * z1 ** 2 * s - g * sin(theta2) + g * sin(theta1) * c)
            + m2 * l2 * z2 ** 2 * s * c
        )
        / l2
        / (m1 + m2 * s ** 2)
    )
    return theta1dot, z1dot, theta2dot, z2dot

def deriv_cas(y, t, l1, l2, m1, m2):
    """Return first deriv. using equations of motion calculated using CAS """
    theta1, z1, theta2, z2 = y

    c, s = cos(theta1 - theta2), sin(theta1 - theta2)

    theta1dot = z1
    theta2dot = z2
    z1dot = -(
        g * m1 * sin(theta1)
        + g * m2 * sin(theta1 - 2 * theta2) / 2
        + g * m2 * sin(theta1) / 2
        + l1 * m2 * z1 ** 2 * sin(2 * theta1 - 2 * theta2) / 2
        + l2 * m2 * z2 ** 2 * sin(theta1 - theta2)
    ) / (l1 * (m1 - m2 * cos(theta1 - theta2) ** 2 + m2))
    z2dot = (
        g * m1 * sin(2 * theta1 - theta2) / 2
        - g * m1 * sin(theta2) / 2
        + g * m2 * sin(2 * theta1 - theta2) / 2
        - g * m2 * sin(theta2) / 2
        + l1 * m1 * z1 ** 2 * sin(theta1 - theta2)
        + l1 * m2 * z1 ** 2 * sin(theta1 - theta2)
        + l2 * m2 * z2 ** 2 * sin(2 * theta1 - 2 * theta2) / 2
    ) / (l2 * (m1 - m2 * cos(theta1 - theta2) ** 2 + m2))
    return theta1dot, z1dot, theta2dot, z2dot

def calc_E(y):
    """Return the total energy of the system."""

    th1, th1d, th2, th2d = y.T
    V = -(m1 + m2) * l1 * g * np.cos(th1) - m2 * l2 * g * np.cos(th2)
    T = 0.5 * m1 * (l1 * th1d) ** 2 + 0.5 * m2 * (
        (l1 * th1d) ** 2
        + (l2 * th2d) ** 2
        + 2 * l1 * l2 * th1d * th2d * np.cos(th1 - th2)
    )
    return T + V

def plot_motion(t, y, title="", save=False):
    # Unpack z and theta as a function of time
    theta1, theta2 = y[:, 0], y[:, 2]

    # Convert to Cartesian coordinates of the two bob positions.
    x1 = l1 * np.sin(theta1)
    y1 = -l1 * np.cos(theta1)
    x2 = x1 + l2 * np.sin(theta2)
    y2 = y1 - l2 * np.cos(theta2)

    # Create animated plot
    fig = plt.figure()
    plt.title(title)
    pltsize = 1.2 * (l1 + l2)
    ax = fig.add_subplot(
        111, autoscale_on=False, xlim=(-pltsize, pltsize), ylim=(-pltsize, pltsize)
    )
    plt.axis("off")

    # Trail
    trail, = ax.plot(
        [], [], c="r", solid_capstyle="butt", lw=2, alpha=0.4, linestyle=":"
    )
    # The pendulum rods.
    line, = ax.plot([], [], c="k", lw=2)
    # Circles representing the anchor point of rod 1, and bobs 1 and 2.
    c0 = Circle((0, 0), r / 2, fc="k", zorder=10)
    c1 = Circle((0, 0), r, fc="b", ec="b", zorder=10)
    c2 = Circle((0, 0), r, fc="r", ec="r", zorder=10)
    # Time label
    time_template = "time = %.1fs"
    time_text = ax.text(0.05, 0.9, "", transform=ax.transAxes)

    def animation_init():
        line.set_data([], [])
        ax.add_patch(c0)
        ax.add_patch(c1)
        ax.add_patch(c2)
        trail.set_data([], [])
        time_text.set_text("")
        return line, time_text, c0, c1, c2

    def animation_func(i):
        thisx = [0, x1[i], x2[i]]
        thisy = [0, y1[i], y2[i]]

        line.set_data(thisx, thisy)
        c1.center = (thisx[1], thisy[1])
        c2.center = (thisx[2], thisy[2])
        trail.set_data(x2[1:i], y2[1:i])
        time_text.set_text(time_template % (i * dt))
        return line, time_text, c1, c2, trail

    ani = animation.FuncAnimation(
        fig,
        animation_func,
        range(0, t.size, di),
        interval=fps,
        blit=True,
        init_func=animation_init,
    )

    # Later ffmpeg crop=1246:646:352:560 to limit to actual motion
    if save:
        ani.save(
            "../output/double_pendulum_2d_%s.mp4" % title,
            animation.writers["ffmpeg"](fps=fps),
            dpi=300,
        )

    plt.show()

def plot_comparison(t, y, y_cas):
    plt.figure()
    plt.subplot(321)
    plt.plot(t, y[:, 0])
    plt.title("theta1 lit")
    plt.subplot(322)
    plt.plot(t, y[:, 2])
    plt.title("theta2 lit")
    plt.subplot(323)
    plt.plot(t, y_cas[:, 0])
    plt.title("theta1 cas")
    plt.subplot(324)
    plt.plot(t, y_cas[:, 2])
    plt.title("theta2 cas")
    plt.subplot(325)
    plt.plot(t, y_cas[:, 0] - y[:, 0])
    plt.title("theta1 diff")
    plt.subplot(326)
    plt.plot(t, y_cas[:, 2] - y[:, 2])
    plt.title("theta2 diff")
    plt.tight_layout()
    plt.show()

def check_energy_conservation(y, y0):
    # Check that the calculation conserves total energy to within some tolerance.
    EDRIFT = 0.05
    # Total energy from the initial conditions
    E = calc_E(y0)
    if np.max(np.sum(np.abs(calc_E(y) - E))) > EDRIFT:
        sys.exit("Maximum energy drift of {} exceeded.".format(EDRIFT))

# Do the numerical integration of the equations of motion
y = odeint(deriv, y0, t, args=(l1, l2, m1, m2))
y_cas = odeint(deriv_cas, y0, t, args=(l1, l2, m1, m2))
y_cas_small = odeint(deriv_cas, y0_small, t, args=(l1, l2, m1, m2))

check_energy_conservation(y, y0)
check_energy_conservation(y_cas, y0)

plot_motion(t, y_cas, title="Large Exitation", save=True)
plot_motion(t, y_cas_small, title="Small Exitation", save=True)
plot_comparison(t, y, y_cas)

np.savez(
    "../output/pointmass-double-pendulum-large_exitation-2d-literature_eom",
    t=t,
    y0=y0,
    y=y,
)
np.savez("../output/pointmass-double-pendulum-large_exitation-2d", t=t, y0=y0, y=y_cas)
np.savez(
    "../output/pointmass-double-pendulum-small_exitation-2d",
    t=t,
    y0=y0_small,
    y=y_cas_small,
)

Figure 91: Validation of the equations of motion generated using the above SymPy code against those taken from [68]. Note how symbolically equal equations can lead to different result due to numerical inaccuracies in computers.

10.2.2 2D Point Mass and Distributed Mass

The following code derives the equations of motion for a double pendulum where the second pendulum is a distributed mass. Note that this assumes that the rotational inertia I2 is provided relative to the axis of rotation lying in m1.

""" EoM for 2d distributed mass double pendulum """
from sympy import symbols, Function, Eq
from sympy import sin, cos
from sympy import diff, simplify, solve
from sympy import init_printing

init_printing()

""" Define Symbols """
l1, l2, m1, m2, I2 = symbols("l1 l2 m1 m2 I2")
t = symbols("t")
g = symbols("g")
L, Pot, Kin = symbols("L Pot Kin")
testPot, testKin = symbols("testPot testKin")
theta1 = Function("theta1")(t)
theta2 = Function("theta2")(t)
z1, z2, zdot1, zdot2 = symbols("z1 z2 zdot1 zdot2")
x1, y1, x2, y2 = symbols("x1 y1 x2 y2")
q = [theta1, theta2]
substitutions = [
    (theta1.diff(t).diff(t), zdot1),
    (theta2.diff(t).diff(t), zdot2),
    (theta1.diff(t), z1),
    (theta2.diff(t), z2),
]

""" Define Kinematic Relations """
x1 = l1 * sin(theta1)
y1 = -l1 * cos(theta1)
x2 = l1 * sin(theta1) + l2 * sin(theta2)
y2 = -l1 * cos(theta1) - l2 * cos(theta2)

""" Langrangian """
Pot = m1 * g * y1 + m2 * g * y2
Kin = (
    1 / 2 * m1 * (x1.diff(t) ** 2 + y1.diff(t) ** 2)
    + 1 / 2 * m2 * (x2.diff(t) ** 2 + y2.diff(t) ** 2)
    + 1 / 2 * (I2) * theta2.diff(t) ** 2
)
L = Kin - Pot

euleq = [Eq(0, L.diff(qi.diff(t)).diff(t) - L.diff(qi)).simplify() for qi in q]
euleq = [euleqi.subs(substitutions) for euleqi in euleq]

solvedeuleq = solve(euleq, [zdot1, zdot2], simplify=False, rational=False)

for ddot, eqn in solvedeuleq.items():
    print(ddot, eqn.simplify())

zdot1 1.0*(-l2**2*m2**2*(g*sin(theta2(t)) - l1*z1**2*sin(theta1(t) - theta2(t)))*cos(theta1(t) - theta2(t)) + (I2 + l2**2*m2)*(g*m1*sin(theta1(t)) + g*m2*sin(theta1(t)) + l2*m2*z2**2*sin(theta1(t) - theta2(t))))/(l1*(l2**2*m2**2*cos(theta1(t) - theta2(t))**2 - (I2 + l2**2*m2)*(m1 + m2)))
zdot2 1.0*l2*m2*((m1 + m2)*(g*sin(theta2(t)) - l1*z1**2*sin(theta1(t) - theta2(t))) - (g*m1*sin(theta1(t)) + g*m2*sin(theta1(t)) + l2*m2*z2**2*sin(theta1(t) - theta2(t)))*cos(theta1(t) - theta2(t)))/(l2**2*m2**2*cos(theta1(t) - theta2(t))**2 - (I2 + l2**2*m2)*(m1 + m2))

10.2.3 2D Double Pendulum with Controller

The following code extends the python simulation to include the PDα\alpha controller by including it in the equations of motion. It also allows for the visualization of the load as a moving and rotating box. The code was also cleaned up in comparison to the other simulation code.

"""Distributed Mass Double-Pendulum with PDalpha Controller
"""
import os
import sys
from math import pi, sin, cos
import numpy as np
from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt
import seaborn as sns
from matplotlib import rcParams

""" Simulation Setup """
lab_setup = [5, 1, 1, 10, 1 / 2 * 10 * 0.4 ** 2 + 10 * 1 ** 2]  # l1, l2  # m1, m2  # I2
l1_24 = [19, 2 * 2, 50, 2500, 16484]  # l1, l2  # m1, m2  # I2
ecb380 = [83, 3.7 * 2, 300, 15660, 350877]  # l1, l2  # m1, m2  # I2
load_dims = [(0.4, 0.4), (5.1, 2.0), (9.3, 3.7)]
# PDalpha Controller Values
kp, kd, kalpha = 1.0, 4.0, 0.5
# The gravitational acceleration (m.s-2).
GRAVITY = 9.81
# Maximum time, time point spacings and the time grid (all in s).
TMAX, DT = 25, 0.01
TIMESTEPS = np.arange(0, TMAX + DT, DT)
# Initial conditions: theta1, dtheta1/dt, theta2, dtheta2/dt.
y0_small = np.array(
    [np.pi / 18, 0, np.pi / 18, 0, 0]  # theta1  # dtheta1  # theta2  # dtheta2  # tau
)

""" Use passed args as savepath, otherwise only show plots """
workdir, _ = os.path.split(os.path.abspath(__file__))
if len(sys.argv) == 1:
    output = False
else:
    output = os.path.abspath(sys.argv[1])
os.chdir(workdir)

def deriv(t, y, l1, l2, m1, m2, I2, kp, kd, kalpha):
    """Return the first derivatives of y = theta1, z1, theta2, z2."""
    kp *= I2
    kd *= I2
    theta1, z1, theta2, z2, tauprev = y
    error = (0 - theta2) + (0 - theta1) * kalpha
    errordot = (0 - z2) + (0 - z1) * kalpha
    tau = kp * error + kd * errordot
    taudot = (tau - tauprev) / DT

    theta1dot = z1
    theta2dot = z2
    z1dot = (
        1.0
        * (
            -l2 ** 2
            * m2 ** 2
            * (GRAVITY * sin(theta2) - l1 * z1 ** 2 * sin(theta1 - theta2))
            * cos(theta1 - theta2)
            + (I2 + l2 ** 2 * m2)
            * (
                GRAVITY * m1 * sin(theta1)
                + GRAVITY * m2 * sin(theta1)
                + l2 * m2 * z2 ** 2 * sin(theta1 - theta2)
            )
        )
        / (
            l1
            * (
                l2 ** 2 * m2 ** 2 * cos(theta1 - theta2) ** 2
                - (I2 + l2 ** 2 * m2) * (m1 + m2)
            )
        )
    )
    z2dot = (
        1.0
        * l2
        * m2
        * (
            (m1 + m2) * (GRAVITY * sin(theta2) - l1 * z1 ** 2 * sin(theta1 - theta2))
            - (
                GRAVITY * m1 * sin(theta1)
                + GRAVITY * m2 * sin(theta1)
                + l2 * m2 * z2 ** 2 * sin(theta1 - theta2)
            )
            * cos(theta1 - theta2)
        )
        / (
            l2 ** 2 * m2 ** 2 * cos(theta1 - theta2) ** 2
            - (I2 + l2 ** 2 * m2) * (m1 + m2)
        )
    )
    z2dot += tau / I2
    return theta1dot, z1dot, theta2dot, z2dot, taudot

def slugify(value):
    import re

    value = re.sub(r"[^\w\s-]", "", value).strip().lower()
    value = re.sub(r"[-\s]+", "-", value)
    return value

def animated_pendulum(data, params, dims, title="", save=False, show=True):
    import matplotlib.pyplot as plt
    import matplotlib.animation as animation
    from matplotlib.patches import Circle

    # Unpack data and params
    t = data["t"]
    theta1 = data["theta1"]
    theta2 = data["theta2"]
    l1, l2 = params[0:2]
    w, h = dims

    # Plot settings
    r = 0.1 * l2  # bob circle radius
    fps = 25
    di = int(1 / fps / DT)

    # Convert to Cartesian coordinates of the two bob positions.
    x1 = l1 * np.sin(theta1)
    y1 = -l1 * np.cos(theta1)
    x2 = x1 + l2 * np.sin(theta2)
    y2 = y1 - l2 * np.cos(theta2)

    # Create animated plot
    fig = plt.figure()
    plt.title(title)
    pltsize = 1.2 * (l1 + l2)
    ax = fig.add_subplot(
        111,
        autoscale_on=False,
        ylim=(-pltsize, 0.05 * pltsize),
        xlim=(-pltsize * 0.5, pltsize * 0.5),
    )

    def load_vertices(x, y, theta):
        vx0 = np.array([-w, +w, +w, -w, -w])
        vy0 = np.array([-h, -h, +h, +h, -h])
        vx = vx0 * cos(theta) - vy0 * sin(theta)
        vy = vx0 * sin(theta) + vy0 * cos(theta)
        return vx + x, vy + y

    # Trail
    trail, = ax.plot(
        [], [], c="r", solid_capstyle="butt", lw=2, alpha=0.4, linestyle=":"
    )
    # The pendulum rods.
    line, = ax.plot([], [], c="k", lw=1.5)
    # Circles representing the anchor point of rod 1, and bobs 1 and 2.
    c0 = Circle((0, 0), r / 2, fc="k", zorder=10)
    c1 = Circle((0, 0), r, fc="b", ec="b", zorder=10)
    c2 = Circle((0, 0), r, fc="r", ec="r", zorder=10)
    # Box representing the load
    load, = ax.plot([], [], c="r", lw=2)
    # Scale Bar
    ax.plot([-pltsize * 0.2, -pltsize * 0.2], [0, -5], c="gray", lw=5)
    ax.text(-pltsize * 0.2 + pltsize * 0.01, -5, "3m")
    # Time label
    time_template = "time = %.1fs"
    time_text = ax.text(0.05, 0.9, "", transform=ax.transAxes)

    def animation_init():
        line.set_data([], [])
        ax.add_patch(c0)
        ax.add_patch(c1)
        ax.add_patch(c2)
        trail.set_data([], [])
        load.set_data([], [])
        time_text.set_text("")
        return line, time_text, c0, c1, c2, load

    def animation_func(i):
        thisx = [0, x1[i], x2[i]]
        thisy = [0, y1[i], y2[i]]
        thistheta = theta2[i]
        vx, vy = load_vertices(x2[i], y2[i], thistheta)

        line.set_data(thisx, thisy)
        c1.center = (thisx[1], thisy[1])
        c2.center = (thisx[2], thisy[2])
        load.set_data(vx, vy)
        trail.set_data(x2[1:i], y2[1:i])
        time_text.set_text(time_template % (i * DT))
        return line, time_text, c1, c2, trail, load

    ani = animation.FuncAnimation(
        fig,
        animation_func,
        range(0, t.size, di),
        interval=fps,
        blit=True,
        init_func=animation_init,
    )

    plt.axis("off")
    # Later ffmpeg crop=1246:646:352:560 to limit to actual motion
    if save:
        ani.save(
            "%s.mp4" % slugify(title), animation.writers["ffmpeg"](fps=fps), dpi=300
        )
    if show:
        plt.show()

def solve(init_cond, params):
    sol = solve_ivp(
        lambda t, y: deriv(t, y, *params), (0, TMAX), init_cond, t_eval=TIMESTEPS
    )
    data = {
        "t": sol.t,
        "theta1": sol.y[0, :],
        "theta2": sol.y[2, :],
        "dtheta1": sol.y[1, :],
        "dtheta2": sol.y[3, :],
        "tau": sol.y[4, :],
    }
    return data

def plot_pos_vel(data, ax, title=""):
    colors = ["C0", "C1", "C2", "C3"]
    linetypes = ["-", "--", "-.", ":"]
    labels2d = [
        r"$\theta_{1} [^\circ]$",
        r"$\dot{\theta}_{1} [\frac{rad}{s}]$",
        r"$\theta_{2} [^\circ]$",
        r"$\dot{\theta}_{2} [\frac{rad}{s}]$",
    ]
    varnames = ["theta1", "dtheta1", "theta2", "dtheta2"]
    for i, vn in enumerate(varnames):
        ax.plot(
            data["t"],
            data[vn] * 180 / pi,
            linetypes[i],
            color=colors[i],
            label=labels2d[i],
            linewidth="0.85",
        )
    if title:
        ax.set_title(title, loc="left")

def plot_torque(data, ax, title=""):
    ax.plot(data["t"], data["tau"], label=r"$\tau [Nm]$", linewidth="1.0")
    if title:
        ax.set_title(title, loc="left")

def output_figure(fig, axs, output):
    for ax in axs:
        ax.grid(axis="y")
    axs[0].legend(loc=1, framealpha=1)
    sns.despine(trim=True, offset=2)
    for ax in axs[0:-1]:
        ax.get_xaxis().set_visible(False)
        ax.spines["bottom"].set_visible(False)
        plt.xlabel("Time [s]")
        plt.tight_layout()
    # fig.subplots_adjust(hspace=0.6)
    plt.tight_layout()
    if output:
        plt.savefig(output)
    else:
        plt.show()

""" Run Simulations """
saveanim = True
showanim = not saveanim
cranes = [lab_setup, l1_24, ecb380]
crane_names = [
    "Lab Setup (5m, 10kg)",
    "L1-24 (19m, 2400kg)",
    "380EC-B16 (83m, 15660kg)",
]
crane_sol = []
for c, name, dims in zip(cranes, crane_names, load_dims):
    params = (*c, kp, kd, kalpha)
    sol = solve(y0_small, params)
    crane_sol.append(sol)

    # Create animation when run without args
    # Run before switching on seaborn to avoid slow animation
    if not output:
        animated_pendulum(sol, params, dims, title=name, save=saveanim, show=showanim)

""" Set up Seaborn Plots """
plt.rc("text", usetex=True)
rcParams["text.latex.preamble"] = [
    r"\usepackage{tgheros}",  # helvetica font
    r"\usepackage{sansmath}",  # math-font matching  helvetica
    r"\sansmath"  # actually tell tex to use it!
    r"\usepackage{siunitx}",  # micro symbols
    r"\sisetup{detect-all}",  # force siunitx to use the fonts
]
rcParams["figure.figsize"] = 9, 4
sns.set()
sns.set_style("ticks")
sns.set_context("paper")

""" Make Plots """
fig, axs = plt.subplots(3, 1, sharex=True)
fig.suptitle("Dampening of Selected Cranes")
for sol, name, ax in zip(crane_sol, crane_names, axs):
    plot_pos_vel(sol, ax, title=name)
output_figure(fig, axs, output)

10.2.4 2D Double Pendulum with Controller and Momentum Limit

The following is a crude extension of the equations of motion to include a momentum envelope limit. This is useful for simulating the effect of different sized CMGs on dampening. Note that these equations do not limit the torque dynamics.

"""Distributed Mass Double-Pendulum w. Torque Limited Controller
"""
import os
import sys
from math import pi, sin, cos
import numpy as np
from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt
import seaborn as sns
from matplotlib import rcParams

""" Simulation Setup """
lab_setup = [5, 1, 1, 10, 1 / 2 * 10 * 0.4 ** 2 + 10 * 1 ** 2]  # l1, l2  # m1, m2  # I2
load_dims = (0.4, 0.4)
max_moment = [2.5, 5, 10]
# PDalpha Controller Values
kp, kd, kalpha = 1.0, 4.0, 0.5
# The gravitational acceleration (m.s-2).
GRAVITY = 9.81
# Maximum time, time point spacings and the time grid (all in s).
TMAX, DT = 25, 0.01
TIMESTEPS = np.arange(0, TMAX + DT, DT)
# Initial conditions: theta1, dtheta1/dt, theta2, dtheta2/dt.
y0_small = np.array(
    [
        np.pi / 18,  # theta1
        0,  # dtheta1
        np.pi / 18,  # theta2
        0,  # dtheta2
        0,  # tau
        0,
    ]  # h
)

""" Use passed args as savepath, otherwise only show plots """
workdir, _ = os.path.split(os.path.abspath(__file__))
if len(sys.argv) == 1:
    output = False
else:
    output = os.path.abspath(sys.argv[1])
os.chdir(workdir)

def deriv(t, y, l1, l2, m1, m2, I2, kp, kd, kalpha, h_max):
    """Return the first derivatives of y = theta1, z1, theta2, z2."""
    kp *= I2
    kd *= I2
    theta1, z1, theta2, z2, tauprev, h_prev = y
    error = (0 - theta2) + (0 - theta1) * kalpha
    errordot = (0 - z2) + (0 - z1) * kalpha
    tau = kp * error + kd * errordot
    h_next = h_prev + tau * DT
    if abs(h_next) >= h_max:
        tau = 0
    h_prev += tau * DT
    taudot = (tau - tauprev) / DT
    hdot = tau

    theta1dot = z1
    theta2dot = z2
    z1dot = (
        1.0
        * (
            -l2 ** 2
            * m2 ** 2
            * (GRAVITY * sin(theta2) - l1 * z1 ** 2 * sin(theta1 - theta2))
            * cos(theta1 - theta2)
            + (I2 + l2 ** 2 * m2)
            * (
                GRAVITY * m1 * sin(theta1)
                + GRAVITY * m2 * sin(theta1)
                + l2 * m2 * z2 ** 2 * sin(theta1 - theta2)
            )
        )
        / (
            l1
            * (
                l2 ** 2 * m2 ** 2 * cos(theta1 - theta2) ** 2
                - (I2 + l2 ** 2 * m2) * (m1 + m2)
            )
        )
    )
    z2dot = (
        1.0
        * l2
        * m2
        * (
            (m1 + m2) * (GRAVITY * sin(theta2) - l1 * z1 ** 2 * sin(theta1 - theta2))
            - (
                GRAVITY * m1 * sin(theta1)
                + GRAVITY * m2 * sin(theta1)
                + l2 * m2 * z2 ** 2 * sin(theta1 - theta2)
            )
            * cos(theta1 - theta2)
        )
        / (
            l2 ** 2 * m2 ** 2 * cos(theta1 - theta2) ** 2
            - (I2 + l2 ** 2 * m2) * (m1 + m2)
        )
    )
    z2dot += tau / I2
    return theta1dot, z1dot, theta2dot, z2dot, taudot, hdot

def slugify(value):
    import re

    value = re.sub(r"[^\w\s-]", "", value).strip().lower()
    value = re.sub(r"[-\s]+", "-", value)
    return value

def animated_pendulum(data, params, dims, title="", save=False, show=True):
    import matplotlib.pyplot as plt
    import matplotlib.animation as animation
    from matplotlib.patches import Circle

    # Unpack data and params
    t = data["t"]
    theta1 = data["theta1"]
    theta2 = data["theta2"]
    l1, l2 = params[0:2]
    w, h = dims

    # Plot settings
    r = 0.1 * l2  # bob circle radius
    fps = 25
    di = int(1 / fps / DT)

    # Convert to Cartesian coordinates of the two bob positions.
    x1 = l1 * np.sin(theta1)
    y1 = -l1 * np.cos(theta1)
    x2 = x1 + l2 * np.sin(theta2)
    y2 = y1 - l2 * np.cos(theta2)

    # Create animated plot
    fig = plt.figure()
    plt.title(title)
    pltsize = 1.2 * (l1 + l2)
    ax = fig.add_subplot(
        111,
        autoscale_on=False,
        ylim=(-pltsize, 0.05 * pltsize),
        xlim=(-pltsize * 0.5, pltsize * 0.5),
    )

    def load_vertices(x, y, theta):
        vx0 = np.array([-w, +w, +w, -w, -w])
        vy0 = np.array([-h, -h, +h, +h, -h])
        vx = vx0 * cos(theta) - vy0 * sin(theta)
        vy = vx0 * sin(theta) + vy0 * cos(theta)
        return vx + x, vy + y

    # Trail
    trail, = ax.plot(
        [], [], c="r", solid_capstyle="butt", lw=2, alpha=0.4, linestyle=":"
    )
    # The pendulum rods.
    line, = ax.plot([], [], c="k", lw=1.5)
    # Circles representing the anchor point of rod 1, and bobs 1 and 2.
    c0 = Circle((0, 0), r / 2, fc="k", zorder=10)
    c1 = Circle((0, 0), r, fc="b", ec="b", zorder=10)
    c2 = Circle((0, 0), r, fc="r", ec="r", zorder=10)
    # Box representing the load
    load, = ax.plot([], [], c="r", lw=2)
    # Scale Bar
    ax.plot([-pltsize * 0.2, -pltsize * 0.2], [0, -5], c="gray", lw=5)
    ax.text(-pltsize * 0.2 + pltsize * 0.01, -5, "3m")
    # Time label
    time_template = "time = %.1fs"
    time_text = ax.text(0.05, 0.9, "", transform=ax.transAxes)

    def animation_init():
        line.set_data([], [])
        ax.add_patch(c0)
        ax.add_patch(c1)
        ax.add_patch(c2)
        trail.set_data([], [])
        load.set_data([], [])
        time_text.set_text("")
        return line, time_text, c0, c1, c2, load

    def animation_func(i):
        thisx = [0, x1[i], x2[i]]
        thisy = [0, y1[i], y2[i]]
        thistheta = theta2[i]
        vx, vy = load_vertices(x2[i], y2[i], thistheta)

        line.set_data(thisx, thisy)
        c1.center = (thisx[1], thisy[1])
        c2.center = (thisx[2], thisy[2])
        load.set_data(vx, vy)
        trail.set_data(x2[1:i], y2[1:i])
        time_text.set_text(time_template % (i * DT))
        return line, time_text, c1, c2, trail, load

    ani = animation.FuncAnimation(
        fig,
        animation_func,
        range(0, t.size, di),
        interval=fps,
        blit=True,
        init_func=animation_init,
    )

    plt.axis("off")
    # Later ffmpeg crop=1246:646:352:560 to limit to actual motion
    if save:
        ani.save(
            "%s.mp4" % slugify(title), animation.writers["ffmpeg"](fps=fps), dpi=300
        )
    if show:
        plt.show()

def solve(init_cond, params):
    sol = solve_ivp(
        lambda t, y: deriv(t, y, *params), (0, TMAX), init_cond, t_eval=TIMESTEPS
    )
    data = {
        "t": sol.t,
        "theta1": sol.y[0, :],
        "theta2": sol.y[2, :],
        "dtheta1": sol.y[1, :],
        "dtheta2": sol.y[3, :],
        "tau": sol.y[4, :],
    }
    return data

def plot_pos_vel(data, ax, title=""):
    colors = ["C0", "C1", "C2", "C3"]
    linetypes = ["-", "--", "-.", ":"]
    labels2d = [
        r"$\theta_{1} [^\circ]$",
        r"$\dot{\theta}_{1} [\frac{rad}{s}]$",
        r"$\theta_{2} [^\circ]$",
        r"$\dot{\theta}_{2} [\frac{rad}{s}]$",
    ]
    varnames = ["theta1", "dtheta1", "theta2", "dtheta2"]
    for i, vn in enumerate(varnames):
        ax.plot(
            data["t"],
            data[vn] * 180 / pi,
            linetypes[i],
            color=colors[i],
            label=labels2d[i],
            linewidth="0.85",
        )
    if title:
        ax.set_title(title, loc="left")

def plot_axis_and_torque(data, axs, cid, title=""):
    colors = ["C0", "C1", "C2", "C3"]
    trim_data = 5
    t = data["t"][trim_data:]
    tau = data["tau"][trim_data:]
    theta = data["theta2"][trim_data:]
    workspace = np.cumsum(tau) * DT
    for ax, y in zip(axs, [theta, tau, workspace]):
        ax.plot(t, y, label=title, color=colors[cid], linewidth="0.85")

def fmt_axcol(axs):
    for ax in axs:
        ax.grid(axis="y")
    for ax in axs[0:-1]:
        ax.get_xaxis().set_visible(False)
        ax.spines["bottom"].set_visible(False)
    axs[-1].set_xlabel("Time [s]")
    axs[0].legend(loc=1, framealpha=1)
    # fig.subplots_adjust(hspace=0.6)

def output_figure(output):
    sns.despine(trim=True, offset=2)
    plt.tight_layout()
    if output:
        plt.savefig(output)
    else:
        plt.show()

""" Run Simulations """
saveanim = True
showanim = not saveanim
titles = ["h-max: {} Nms".format(h) for h in max_moment]
crane_sol = []
for h, name in zip(max_moment, titles):
    h_cur = 0
    params = (*lab_setup, kp, kd, kalpha, h)
    sol = solve(y0_small, params)
    crane_sol.append(sol)

    # Create animation when run without args
    # Run before switching on seaborn to avoid slow animation
    if not output:
        animated_pendulum(
            sol, params, load_dims, title=name, save=saveanim, show=showanim
        )

""" Set up Seaborn Plots """
plt.rc("text", usetex=True)
rcParams["text.latex.preamble"] = [
    r"\usepackage{tgheros}",  # helvetica font
    r"\usepackage{sansmath}",  # math-font matching  helvetica
    r"\sansmath"  # actually tell tex to use it!
    r"\usepackage{siunitx}",  # micro symbols
    r"\sisetup{detect-all}",  # force siunitx to use the fonts
]
rcParams["figure.figsize"] = 9, 4
sns.set()
sns.set_style("ticks")
sns.set_context("paper")

""" Make Torque Plots """
fig, axs = plt.subplots(3, 3, sharex=True)
for sol, name, axcol, cid in zip(crane_sol, titles, axs.T, range(3)):
    plot_axis_and_torque(sol, axcol, cid, title=name)
    fmt_axcol(axcol)
labels = [r"$\theta_2 [^\circ]$", r"$\tau [Nm]$", r"h [Nms]"]
for ax, lbl in zip(axs.T[0], labels):
    ax.set_ylabel(lbl, rotation="horizontal")
    ax.yaxis.set_label_coords(-0.1, 1.02)
output_figure(output)

10.3 3D Point Mass Double Pendulum

"""Simulate 3D Point Mass Double-Pendulum

Uses better ode solver than the original 2d code its based off.
"""
import sys
import numpy as np
from math import isclose, sin, cos
from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from matplotlib.patches import Circle
from mpl_toolkits.mplot3d import Axes3D

# Pendulum rod lengths (m), bob masses (kg).
l1, l2 = 1, 1
m1, m2 = 1, 1
# The gravitational acceleration (m.s-2).
g = 9.81
# Maximum time, time point spacings and the time grid (all in s).
tmax, dt = 30, 0.01
t = np.arange(0, tmax + dt, dt)
# Initial conditions: theta1, dtheta1/dt, theta2, dtheta2/dt.
y0_large = np.array(
    [
        np.pi / 3,  #  theta11
        0,  # dtheta11
        np.pi / 3,  #  theta12
        0,  # dtheta12
        np.pi / 3,  #  theta21
        0,  # dtheta22
        np.pi / 3,  #  theta22
        0,  # dtheta22
    ]
)
y0_small = np.array(
    [
        np.pi / 8,  #  theta11
        0,  # dtheta11
        np.pi / 8,  #  theta12
        0,  # dtheta12
        np.pi / 8,  #  theta21
        0,  # dtheta22
        np.pi / 8,  #  theta22
        0,  # dtheta22
    ]
)

# Plot settings
r = 0.05  # bob circle radius
fps = 25
di = int(1 / fps / dt)

def deriv(t, y, l1, l2, m1, m2):
    """Return the first derivatives of y = theta_ij, dtheta_ij"""
    theta11, u11, theta12, u12, theta21, u21, theta22, u22 = y

    dtheta11 = u11
    dtheta12 = u12
    dtheta21 = u21
    dtheta22 = u22
    du11 = (
        m2
        * (
            sin(theta11) * cos(theta12 - theta22) * cos(theta21)
            - sin(theta21) * cos(theta11)
        )
        * (
            g * m1 * sin(theta12)
            + g * m2 * sin(theta12)
            - 2 * l1 * m1 * u11 * u12 * sin(theta11)
            - 2 * l1 * m2 * u11 * u12 * sin(theta11)
            + l2 * m2 * u21 ** 2 * sin(theta12 - theta22) * cos(theta21)
            - 2 * l2 * m2 * u21 * u22 * sin(theta21) * cos(theta12 - theta22)
            + l2 * m2 * u22 ** 2 * sin(theta12 - theta22) * cos(theta21)
        )
        * sin(theta12 - theta22)
        * cos(theta21)
        - m2
        * (
            -m1 * sin(theta11)
            - m2 * sin(theta11) * cos(theta21) ** 2
            + m2 * sin(theta21) * cos(theta12 - theta22) * cos(theta11) * cos(theta21)
        )
        * (
            -g * sin(theta22)
            + l1 * u11 ** 2 * sin(theta12 - theta22) * cos(theta11)
            + 2 * l1 * u11 * u12 * sin(theta11) * cos(theta12 - theta22)
            + l1 * u12 ** 2 * sin(theta12 - theta22) * cos(theta11)
            + 2 * l2 * u21 * u22 * sin(theta21)
        )
        * sin(theta12 - theta22)
        - m2
        * (
            m2
            * (
                sin(theta11) * sin(theta21) * cos(theta12 - theta22)
                + cos(theta11) * cos(theta21)
            )
            * cos(theta12 - theta22) ** 2
            + m2
            * sin(theta12 - theta22) ** 2
            * sin(theta11)
            * sin(theta21)
            * cos(theta12 - theta22)
            + (-m1 - m2)
            * (
                sin(theta11) * sin(theta21) * cos(theta12 - theta22)
                + cos(theta11) * cos(theta21)
            )
        )
        * (
            g * sin(theta21) * cos(theta22)
            - l1 * u11 ** 2 * sin(theta11) * cos(theta21)
            + l1 * u11 ** 2 * sin(theta21) * cos(theta12 - theta22) * cos(theta11)
            - 2 * l1 * u11 * u12 * sin(theta12 - theta22) * sin(theta11) * sin(theta21)
            + l1 * u12 ** 2 * sin(theta21) * cos(theta12 - theta22) * cos(theta11)
            + l2 * u22 ** 2 * sin(2 * theta21) / 2
        )
        + (
            -m1
            + m2 * sin(theta12 - theta22) ** 2 * sin(theta21) ** 2
            + m2 * cos(theta12 - theta22) ** 2
            - m2
        )
        * (
            g * m1 * sin(theta11) * cos(theta12)
            + g * m2 * sin(theta11) * cos(theta12)
            + l1 * m1 * u12 ** 2 * sin(2 * theta11) / 2
            + l1 * m2 * u12 ** 2 * sin(2 * theta11) / 2
            + l2 * m2 * u21 ** 2 * sin(theta11) * cos(theta12 - theta22) * cos(theta21)
            - l2 * m2 * u21 ** 2 * sin(theta21) * cos(theta11)
            + 2
            * l2
            * m2
            * u21
            * u22
            * sin(theta12 - theta22)
            * sin(theta11)
            * sin(theta21)
            + l2 * m2 * u22 ** 2 * sin(theta11) * cos(theta12 - theta22) * cos(theta21)
        )
    ) / (
        l1
        * m1
        * (
            m1
            - 2
            * m2
            * sin(theta11)
            * sin(theta21)
            * cos(theta12 - theta22)
            * cos(theta11)
            * cos(theta21)
            - 2
            * m2
            * sin(theta12)
            * sin(theta22)
            * cos(theta12 - theta22)
            * cos(theta11) ** 2
            * cos(theta21) ** 2
            - m2 * cos(theta11) ** 2 * cos(theta12) ** 2 * cos(theta21) ** 2
            - m2 * cos(theta11) ** 2 * cos(theta21) ** 2 * cos(theta22) ** 2
            + m2 * cos(theta11) ** 2
            + m2 * cos(theta21) ** 2
        )
    )
    du12 = (
        m2
        * (
            sin(theta11) * cos(theta12 - theta22) * cos(theta21)
            - sin(theta21) * cos(theta11)
        )
        * (
            g * m1 * sin(theta11) * cos(theta12)
            + g * m2 * sin(theta11) * cos(theta12)
            + l1 * m1 * u12 ** 2 * sin(2 * theta11) / 2
            + l1 * m2 * u12 ** 2 * sin(2 * theta11) / 2
            + l2 * m2 * u21 ** 2 * sin(theta11) * cos(theta12 - theta22) * cos(theta21)
            - l2 * m2 * u21 ** 2 * sin(theta21) * cos(theta11)
            + 2
            * l2
            * m2
            * u21
            * u22
            * sin(theta12 - theta22)
            * sin(theta11)
            * sin(theta21)
            + l2 * m2 * u22 ** 2 * sin(theta11) * cos(theta12 - theta22) * cos(theta21)
        )
        * sin(theta12 - theta22)
        * cos(theta21)
        - m2
        * (
            -m1 * sin(theta21)
            + m2 * sin(theta11) * cos(theta12 - theta22) * cos(theta11) * cos(theta21)
            - m2 * sin(theta21) * cos(theta11) ** 2
        )
        * (
            g * sin(theta21) * cos(theta22)
            - l1 * u11 ** 2 * sin(theta11) * cos(theta21)
            + l1 * u11 ** 2 * sin(theta21) * cos(theta12 - theta22) * cos(theta11)
            - 2 * l1 * u11 * u12 * sin(theta12 - theta22) * sin(theta11) * sin(theta21)
            + l1 * u12 ** 2 * sin(theta21) * cos(theta12 - theta22) * cos(theta11)
            + l2 * u22 ** 2 * sin(2 * theta21) / 2
        )
        * sin(theta12 - theta22)
        + m2
        * (
            m2
            * (
                sin(theta11) * sin(theta21) * cos(theta12 - theta22)
                + cos(theta11) * cos(theta21)
            )
            ** 2
            * cos(theta12 - theta22)
            + m2
            * (
                sin(theta11) * sin(theta21) * cos(theta12 - theta22)
                + cos(theta11) * cos(theta21)
            )
            * sin(theta12 - theta22) ** 2
            * sin(theta11)
            * sin(theta21)
            + (-m1 - m2) * cos(theta12 - theta22)
        )
        * (
            -g * sin(theta22)
            + l1 * u11 ** 2 * sin(theta12 - theta22) * cos(theta11)
            + 2 * l1 * u11 * u12 * sin(theta11) * cos(theta12 - theta22)
            + l1 * u12 ** 2 * sin(theta12 - theta22) * cos(theta11)
            + 2 * l2 * u21 * u22 * sin(theta21)
        )
        + (
            -m1
            + m2
            * (
                sin(theta11) * sin(theta21) * cos(theta12 - theta22)
                + cos(theta11) * cos(theta21)
            )
            ** 2
            + m2 * sin(theta12 - theta22) ** 2 * sin(theta11) ** 2
            - m2
        )
        * (
            g * m1 * sin(theta12)
            + g * m2 * sin(theta12)
            - 2 * l1 * m1 * u11 * u12 * sin(theta11)
            - 2 * l1 * m2 * u11 * u12 * sin(theta11)
            + l2 * m2 * u21 ** 2 * sin(theta12 - theta22) * cos(theta21)
            - 2 * l2 * m2 * u21 * u22 * sin(theta21) * cos(theta12 - theta22)
            + l2 * m2 * u22 ** 2 * sin(theta12 - theta22) * cos(theta21)
        )
    ) / (
        l1
        * m1
        * (
            m1
            - 2
            * m2
            * sin(theta11)
            * sin(theta21)
            * cos(theta12 - theta22)
            * cos(theta11)
            * cos(theta21)
            - 2
            * m2
            * sin(theta12)
            * sin(theta22)
            * cos(theta12 - theta22)
            * cos(theta11) ** 2
            * cos(theta21) ** 2
            - m2 * cos(theta11) ** 2 * cos(theta12) ** 2 * cos(theta21) ** 2
            - m2 * cos(theta11) ** 2 * cos(theta21) ** 2 * cos(theta22) ** 2
            + m2 * cos(theta11) ** 2
            + m2 * cos(theta21) ** 2
        )
        * cos(theta11)
    )
    du21 = (
        m2
        * (m1 + m2)
        * (
            -sin(theta11) * cos(theta21)
            + sin(theta21) * cos(theta12 - theta22) * cos(theta11)
        )
        * (
            -g * sin(theta22)
            + l1 * u11 ** 2 * sin(theta12 - theta22) * cos(theta11)
            + 2 * l1 * u11 * u12 * sin(theta11) * cos(theta12 - theta22)
            + l1 * u12 ** 2 * sin(theta12 - theta22) * cos(theta11)
            + 2 * l2 * u21 * u22 * sin(theta21)
        )
        * sin(theta12 - theta22)
        * cos(theta11)
        + (m1 + m2)
        * (
            -m1
            + m2 * sin(theta12 - theta22) ** 2 * sin(theta11) ** 2
            + m2 * cos(theta12 - theta22) ** 2
            - m2
        )
        * (
            g * sin(theta21) * cos(theta22)
            - l1 * u11 ** 2 * sin(theta11) * cos(theta21)
            + l1 * u11 ** 2 * sin(theta21) * cos(theta12 - theta22) * cos(theta11)
            - 2 * l1 * u11 * u12 * sin(theta12 - theta22) * sin(theta11) * sin(theta21)
            + l1 * u12 ** 2 * sin(theta21) * cos(theta12 - theta22) * cos(theta11)
            + l2 * u22 ** 2 * sin(2 * theta21) / 2
        )
        - (
            -m1 * sin(theta21)
            + m2 * sin(theta11) * cos(theta12 - theta22) * cos(theta11) * cos(theta21)
            - m2 * sin(theta21) * cos(theta11) ** 2
        )
        * (
            g * m1 * sin(theta12)
            + g * m2 * sin(theta12)
            - 2 * l1 * m1 * u11 * u12 * sin(theta11)
            - 2 * l1 * m2 * u11 * u12 * sin(theta11)
            + l2 * m2 * u21 ** 2 * sin(theta12 - theta22) * cos(theta21)
            - 2 * l2 * m2 * u21 * u22 * sin(theta21) * cos(theta12 - theta22)
            + l2 * m2 * u22 ** 2 * sin(theta12 - theta22) * cos(theta21)
        )
        * sin(theta12 - theta22)
        - (
            m2
            * (
                sin(theta11) * sin(theta21) * cos(theta12 - theta22)
                + cos(theta11) * cos(theta21)
            )
            * cos(theta12 - theta22) ** 2
            + m2
            * sin(theta12 - theta22) ** 2
            * sin(theta11)
            * sin(theta21)
            * cos(theta12 - theta22)
            + (-m1 - m2)
            * (
                sin(theta11) * sin(theta21) * cos(theta12 - theta22)
                + cos(theta11) * cos(theta21)
            )
        )
        * (
            g * m1 * sin(theta11) * cos(theta12)
            + g * m2 * sin(theta11) * cos(theta12)
            + l1 * m1 * u12 ** 2 * sin(2 * theta11) / 2
            + l1 * m2 * u12 ** 2 * sin(2 * theta11) / 2
            + l2 * m2 * u21 ** 2 * sin(theta11) * cos(theta12 - theta22) * cos(theta21)
            - l2 * m2 * u21 ** 2 * sin(theta21) * cos(theta11)
            + 2
            * l2
            * m2
            * u21
            * u22
            * sin(theta12 - theta22)
            * sin(theta11)
            * sin(theta21)
            + l2 * m2 * u22 ** 2 * sin(theta11) * cos(theta12 - theta22) * cos(theta21)
        )
    ) / (
        l2
        * m1
        * (
            m1
            - 2
            * m2
            * sin(theta11)
            * sin(theta21)
            * cos(theta12 - theta22)
            * cos(theta11)
            * cos(theta21)
            - 2
            * m2
            * sin(theta12)
            * sin(theta22)
            * cos(theta12 - theta22)
            * cos(theta11) ** 2
            * cos(theta21) ** 2
            - m2 * cos(theta11) ** 2 * cos(theta12) ** 2 * cos(theta21) ** 2
            - m2 * cos(theta11) ** 2 * cos(theta21) ** 2 * cos(theta22) ** 2
            + m2 * cos(theta11) ** 2
            + m2 * cos(theta21) ** 2
        )
    )
    du22 = (
        -m2
        * (m1 + m2)
        * (
            -sin(theta11) * cos(theta21)
            + sin(theta21) * cos(theta12 - theta22) * cos(theta11)
        )
        * (
            g * sin(theta21) * cos(theta22)
            - l1 * u11 ** 2 * sin(theta11) * cos(theta21)
            + l1 * u11 ** 2 * sin(theta21) * cos(theta12 - theta22) * cos(theta11)
            - 2 * l1 * u11 * u12 * sin(theta12 - theta22) * sin(theta11) * sin(theta21)
            + l1 * u12 ** 2 * sin(theta21) * cos(theta12 - theta22) * cos(theta11)
            + l2 * u22 ** 2 * sin(2 * theta21) / 2
        )
        * sin(theta12 - theta22)
        * cos(theta11)
        + (-m1 - m2)
        * (
            -m1
            + m2
            * (
                sin(theta11) * sin(theta21) * cos(theta12 - theta22)
                + cos(theta11) * cos(theta21)
            )
            ** 2
            + m2 * sin(theta12 - theta22) ** 2 * sin(theta21) ** 2
            - m2
        )
        * (
            -g * sin(theta22)
            + l1 * u11 ** 2 * sin(theta12 - theta22) * cos(theta11)
            + 2 * l1 * u11 * u12 * sin(theta11) * cos(theta12 - theta22)
            + l1 * u12 ** 2 * sin(theta12 - theta22) * cos(theta11)
            + 2 * l2 * u21 * u22 * sin(theta21)
        )
        + (
            -m1 * sin(theta11)
            - m2 * sin(theta11) * cos(theta21) ** 2
            + m2 * sin(theta21) * cos(theta12 - theta22) * cos(theta11) * cos(theta21)
        )
        * (
            g * m1 * sin(theta11) * cos(theta12)
            + g * m2 * sin(theta11) * cos(theta12)
            + l1 * m1 * u12 ** 2 * sin(2 * theta11) / 2
            + l1 * m2 * u12 ** 2 * sin(2 * theta11) / 2
            + l2 * m2 * u21 ** 2 * sin(theta11) * cos(theta12 - theta22) * cos(theta21)
            - l2 * m2 * u21 ** 2 * sin(theta21) * cos(theta11)
            + 2
            * l2
            * m2
            * u21
            * u22
            * sin(theta12 - theta22)
            * sin(theta11)
            * sin(theta21)
            + l2 * m2 * u22 ** 2 * sin(theta11) * cos(theta12 - theta22) * cos(theta21)
        )
        * sin(theta12 - theta22)
        - (
            m2
            * (
                sin(theta11) * sin(theta21) * cos(theta12 - theta22)
                + cos(theta11) * cos(theta21)
            )
            ** 2
            * cos(theta12 - theta22)
            + m2
            * (
                sin(theta11) * sin(theta21) * cos(theta12 - theta22)
                + cos(theta11) * cos(theta21)
            )
            * sin(theta12 - theta22) ** 2
            * sin(theta11)
            * sin(theta21)
            + (-m1 - m2) * cos(theta12 - theta22)
        )
        * (
            g * m1 * sin(theta12)
            + g * m2 * sin(theta12)
            - 2 * l1 * m1 * u11 * u12 * sin(theta11)
            - 2 * l1 * m2 * u11 * u12 * sin(theta11)
            + l2 * m2 * u21 ** 2 * sin(theta12 - theta22) * cos(theta21)
            - 2 * l2 * m2 * u21 * u22 * sin(theta21) * cos(theta12 - theta22)
            + l2 * m2 * u22 ** 2 * sin(theta12 - theta22) * cos(theta21)
        )
    ) / (
        l2
        * m1
        * (
            m1
            - 2
            * m2
            * sin(theta11)
            * sin(theta21)
            * cos(theta12 - theta22)
            * cos(theta11)
            * cos(theta21)
            - 2
            * m2
            * sin(theta12)
            * sin(theta22)
            * cos(theta12 - theta22)
            * cos(theta11) ** 2
            * cos(theta21) ** 2
            - m2 * cos(theta11) ** 2 * cos(theta12) ** 2 * cos(theta21) ** 2
            - m2 * cos(theta11) ** 2 * cos(theta21) ** 2 * cos(theta22) ** 2
            + m2 * cos(theta11) ** 2
            + m2 * cos(theta21) ** 2
        )
        * cos(theta21)
    )

    return [dtheta11, du11, dtheta12, du12, dtheta21, du21, dtheta22, du22]

zero_crossings = [
    lambda t, y: sin(y[0]),  # theta11
    lambda t, y: sin(y[4]),  # theta21
    lambda t, y: cos(y[2]),  # theta12
    lambda t, y: cos(y[6]),  # theta22
    lambda t, y: y[1],  # dtheta11
    lambda t, y: y[5],
]  # dtheta21

def plot_motion(t, y, title="", save=False):
    y = np.array(y).T
    theta11 = y[:, 0]
    theta12 = y[:, 2]
    theta21 = y[:, 4]
    theta22 = y[:, 6]

    # Convert to Cartesian coordinates of the two bob positions.
    x1 = l1 * np.sin(theta11)
    y1 = l1 * np.cos(theta11) * np.sin(theta12)
    z1 = -l1 * np.cos(theta11) * np.cos(theta12)
    x2 = x1 + l2 * np.sin(theta21)
    y2 = y1 + l2 * np.cos(theta21) * np.sin(theta22)
    z2 = z1 - l2 * np.cos(theta21) * np.cos(theta22)

    length1 = (isclose(np.linalg.norm([x, y, z]), l1) for x, y, z in zip(x1, y1, z1))
    length2 = (
        isclose(np.linalg.norm([x, y, z]), l2)
        for x, y, z in zip(x2 - x1, y2 - y1, z2 - z1)
    )
    if not (all(length1) and all(length2)):
        print("ERROR: Lengths of links are not maintained!")
        sys.exit()

    # Create animated plot
    fig = plt.figure(figsize=(12, 4))
    pltsize = 1.2 * (l1 + l2)
    axs, lines, trails, circles = [], [], [], []
    for i in range(3):
        ax = fig.add_subplot(
            131 + i,
            autoscale_on=False,
            xlim=(-pltsize, pltsize),
            ylim=(-pltsize, pltsize),
        )

        # Trail
        trail, = ax.plot(
            [], [], c="r", solid_capstyle="butt", lw=2, alpha=0.4, linestyle=":"
        )
        # The pendulum rods.
        line, = ax.plot([], [], c="k", lw=2)
        # Circles representing the anchor point of rod 1, and bobs 1 and 2.
        c0 = Circle((0, 0), r / 2, fc="k", zorder=10)
        c1 = Circle((0, 0), r, fc="b", ec="b", zorder=10)
        c2 = Circle((0, 0), r, fc="r", ec="r", zorder=10)
        for col, single in zip(
            [axs, lines, trails, circles], [ax, line, trail, [c0, c1, c2]]
        ):
            col.append(single)
    # Time label
    time_template = "time = %.1fs"
    time_text = axs[0].text(0.05, 0.9, "", transform=ax.transAxes)
    plt.tight_layout()

    def animation_init():
        for ax, line, trail, circs in zip(axs, lines, trails, circles):
            line.set_data([], [])
            for c in circs:
                ax.add_patch(c)
            trail.set_data([], [])
            time_text.set_text("")
        return axs + lines + trails + [c for cs in circles for c in cs] + [time_text]

    def animation_func(i):
        front = {"x1": x1, "x2": x2, "y1": z1, "y2": z2}
        right = {"x1": y1, "x2": y2, "y1": z1, "y2": z2}
        top = {"x1": x1, "x2": x2, "y1": y1, "y2": y2}
        views = [front, right, top]

        for view, ax, line, trail, circs in zip(views, axs, lines, trails, circles):
            thisx = [0, view["x1"][i], view["x2"][i]]
            thisy = [0, view["y1"][i], view["y2"][i]]
            line.set_data(thisx, thisy)
            circs[1].center = (thisx[1], thisy[1])
            circs[2].center = (thisx[2], thisy[2])
            trail.set_data(view["x2"][1:i], view["y2"][1:i])
        time_text.set_text(time_template % (i * dt))
        return axs + lines + trails + [c for cs in circles for c in cs] + [time_text]

    # blit=True sadly leads to blank animations on saving
    ani = animation.FuncAnimation(
        fig,
        animation_func,
        range(0, len(t), di),
        interval=fps,
        blit=False,
        init_func=animation_init,
    )
    if save:
        ani.save(
            "../output/double_pendulum_3d_%s.mp4" % title.replace(" ", "_"),
            animation.writers["ffmpeg"](fps=fps),
            dpi=300,
        )
    plt.show()

def plot_angles(time, y, events):
    fig, axs = plt.subplots(3, 1, sharex=True)
    colors = ["C0", "C1", "C2", "C3"]
    linetypes = ["-", "--", "-.", ":"]
    labels = {
        "2d": [
            r"$\theta_{1}$",
            r"$\dot{\theta}_{1}$",
            r"$\theta_{2}$",
            r"$\dot{\theta}_{2}$",
        ],
        "3d": [r"$\theta_{11}$", r"$\theta_{12}$", r"$\theta_{21}$", r"$\theta_{22}$"],
        "3ddot": [
            r"$\dot{\theta}_{11}$",
            r"$\dot{\theta}_{12}$",
            r"$\dot{\theta}_{21}$",
            r"$\dot{\theta}_{22}$",
        ],
        "2d3d": [r"$\theta_{1}-\theta_{11}$", r"$\theta_{2}-\theta_{21}$"],
    }

    axs[0].set_title("Polar Angles", loc="left")
    axs[1].set_title("Azimuthal Angles", loc="left")
    axs[2].set_title("Angular Velocites", loc="left")
    for i in range(0, 8, 4):
        polar = y[i, :]
        azimuth = y[i + 2, :]
        axs[0].plot(
            time,
            polar * 180 / np.pi,
            linetypes[i // 2],
            color=colors[i // 2],
            label=labels["3d"][i // 2],
            linewidth="0.85",
        )
        axs[1].plot(
            time,
            azimuth * 180 / np.pi,
            linetypes[i // 2 + 1],
            color=colors[i // 2 + 1],
            label=labels["3d"][i // 2 + 1],
            linewidth="0.85",
        )
    for i, j in zip(range(1, 8, 2), range(4)):
        axs[2].plot(
            time,
            y[i, :] * 180 / np.pi,
            linetypes[j],
            color=colors[j],
            label=labels["3ddot"][j],
            linewidth="0.85",
        )

    # axs[0].scatter(events[0], np.zeros(len(events[0])), marker='x', color=colors[0], label='theta11')
    # axs[0].scatter(events[1], np.zeros(len(events[1])), marker='o', color=colors[2], label='theta21')
    # axs[1].scatter(events[2], np.zeros(len(events[2])), marker='x', color=colors[1], label='theta12')
    # axs[1].scatter(events[3], np.zeros(len(events[3])), marker='o', color=colors[3], label='theta22')
    # axs[2].scatter(events[4], np.zeros(len(events[4])), marker='x', color=colors[0], label='dtheta11')
    # axs[2].scatter(events[5], np.zeros(len(events[5])), marker='o', color=colors[2], label='dtheta21')
    for ax in axs:
        ax.legend(loc=1, framealpha=1)
        ax.grid(axis="y")
    plt.show()

save_animations = True
# Large Exitation
sol = solve_ivp(
    lambda t, y: deriv(t, y, l1, l2, m1, m2),
    (0, tmax),
    y0_large,
    t_eval=t,
    events=zero_crossings,
)
plot_motion(sol.t, sol.y, title="Large Exitation", save=save_animations)
np.savez(
    "../output/pointmass-double-pendulum-large_exitation-3d",
    t=sol.t,
    y0=y0_large,
    y=sol.y,
)

# Small Exitation
sol = solve_ivp(
    lambda t, y: deriv(t, y, l1, l2, m1, m2),
    (0, tmax),
    y0_small,
    t_eval=t,
    events=zero_crossings,
)
plot_angles(t, sol.y, sol.t_events)
plot_motion(sol.t, sol.y, title="Small Exitation", save=save_animations)
np.savez(
    "../output/pointmass-double-pendulum-small_exitation-3d",
    t=sol.t,
    y0=y0_small,
    y=sol.y,
)

10.3.1 Spherical Coordinates

""" EoM of 3d double pendulum using spherical coordinates """
from sympy import symbols, Function, Eq
from sympy import sin, cos
from sympy import diff, simplify, expand, solve
from sympy import init_printing, pprint, latex

init_printing()

""" Define Symbols """
l1, l2, m1, m2 = symbols("l1 l2 m1 m2")
t = symbols("t")
g = symbols("g")
L, Pot, Kin = symbols("L Pot Kin")
theta11 = Function("theta11")(t)
theta12 = Function("theta12")(t)
theta21 = Function("theta21")(t)
theta22 = Function("theta22")(t)
u11, u12, udot11, udot12 = symbols("u11 u12 udot11 udot12")
u21, u22, udot21, udot22 = symbols("u21 u22 udot21 udot22")
x1, y1, z1, x2, y2, z2 = symbols("x1 y1 z1 x2 y2 z1")
q = [theta11, theta12, theta21, theta22]
substitutions = [
    (theta11.diff(t).diff(t), udot11),
    (theta12.diff(t).diff(t), udot12),
    (theta21.diff(t).diff(t), udot21),
    (theta22.diff(t).diff(t), udot22),
    (theta11.diff(t), u11),
    (theta12.diff(t), u12),
    (theta21.diff(t), u21),
    (theta22.diff(t), u22),
]

""" Define Kinematic Relations """
x1 = l1 * sin(theta11) * cos(theta12)
y1 = l1 * sin(theta11) * sin(theta12)
z1 = -l1 * cos(theta11)
x2 = x1 + l2 * sin(theta21) * cos(theta22)
y2 = y1 + l2 * sin(theta21) * sin(theta22)
z2 = z1 - l2 * cos(theta21)

""" Langrangian """
Pot = m1 * g * z1 + m2 * g * z2
Kin = 1 / 2 * m1 * (
    x1.diff(t) ** 2 + y1.diff(t) ** 2 + z1.diff(t) ** 2
) + 1 / 2 * m2 * (x2.diff(t) ** 2 + y2.diff(t) ** 2 + z2.diff(t) ** 2)
L = Kin - Pot

euleq = [Eq(0, L.diff(qi.diff(t)).diff(t) - L.diff(qi)).simplify() for qi in q]
euleq = [euleqi.subs(substitutions) for euleqi in euleq]

solvedeuleq = solve(
    euleq, [udot11, udot12, udot21, udot22], simplify=False, rational=False
)

for ddot, eqn in solvedeuleq.items():
    print(ddot, eqn.simplify())

udot21 1.0*(-m2*(m1 + m2)*(sin(theta11(t))*cos(theta12(t) - theta22(t))*cos(theta21(t)) - sin(theta21(t))*cos(theta11(t)))*(l1*u11**2*sin(theta12(t) - theta22(t))*sin(theta11(t)) - 2*l1*u11*u12*cos(theta12(t) - theta22(t))*cos(theta11(t)) + l1*u12**2*sin(theta12(t) - theta22(t))*sin(theta11(t)) - 2*l2*u21*u22*cos(theta21(t)))*sin(theta12(t) - theta22(t))*sin(theta11(t)) - (m1 + m2)*(-m1 + m2*sin(theta12(t) - theta22(t))**2*cos(theta11(t))**2 + m2*cos(theta12(t) - theta22(t))**2 - m2)*(-1.0*g*sin(theta21(t)) + 1.0*l1*u11**2*sin(theta11(t))*cos(theta12(t) - theta22(t))*cos(theta21(t)) - 1.0*l1*u11**2*sin(theta21(t))*cos(theta11(t)) + 2.0*l1*u11*u12*sin(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)) + 1.0*l1*u12**2*sin(theta11(t))*cos(theta12(t) - theta22(t))*cos(theta21(t)) + 0.5*l2*u22**2*sin(2*theta21(t))) + (-m1*cos(theta21(t)) - m2*sin(theta11(t))**2*cos(theta21(t)) + m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t)))*(2*l1*m1*u11*u12*cos(theta11(t)) + 2*l1*m2*u11*u12*cos(theta11(t)) + l2*m2*u21**2*sin(theta12(t) - theta22(t))*sin(theta21(t)) + 2*l2*m2*u21*u22*cos(theta12(t) - theta22(t))*cos(theta21(t)) + l2*m2*u22**2*sin(theta12(t) - theta22(t))*sin(theta21(t)))*sin(theta12(t) - theta22(t)) - (m2*(sin(theta11(t))*sin(theta21(t)) + cos(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)))*cos(theta12(t) - theta22(t))**2 + m2*sin(theta12(t) - theta22(t))**2*cos(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)) - (m1 + m2)*(sin(theta11(t))*sin(theta21(t)) + cos(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t))))*(1.0*g*m1*sin(theta11(t)) + 1.0*g*m2*sin(theta11(t)) - 0.5*l1*m1*u12**2*sin(2*theta11(t)) - 0.5*l1*m2*u12**2*sin(2*theta11(t)) + 1.0*l2*m2*u21**2*sin(theta11(t))*cos(theta21(t)) - 1.0*l2*m2*u21**2*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t)) + 2.0*l2*m2*u21*u22*sin(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l2*m2*u22**2*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t))))/(l2*m1*(m1 + m2*sin(theta11(t))**2*sin(theta12(t))**2*sin(theta21(t))**2 - 2*m2*sin(theta11(t))**2*sin(theta12(t))*sin(theta21(t))**2*sin(theta22(t))*cos(theta12(t) - theta22(t)) + m2*sin(theta11(t))**2*sin(theta21(t))**2*sin(theta22(t))**2 - 2*m2*sin(theta11(t))**2*sin(theta21(t))**2 + m2*sin(theta11(t))**2 - 2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)) + m2*sin(theta21(t))**2))
udot22 1.0*(16.0*g*m1*cos(2*theta11(t) - theta12(t) + theta22(t)) - 16.0*g*m1*cos(2*theta11(t) + theta12(t) - theta22(t)) + 16.0*g*m2*cos(2*theta11(t) - theta12(t) + theta22(t)) - 16.0*g*m2*cos(2*theta11(t) + theta12(t) - theta22(t)) + 32.0*l1*m1*u11**2*cos(theta11(t) - theta12(t) + theta22(t)) - 32.0*l1*m1*u11**2*cos(theta11(t) + theta12(t) - theta22(t)) + 24.0*l1*m1*u12**2*cos(theta11(t) - theta12(t) + theta22(t)) - 24.0*l1*m1*u12**2*cos(theta11(t) + theta12(t) - theta22(t)) - 8.0*l1*m1*u12**2*cos(3*theta11(t) - theta12(t) + theta22(t)) + 8.0*l1*m1*u12**2*cos(3*theta11(t) + theta12(t) - theta22(t)) + 32.0*l1*m2*u11**2*cos(theta11(t) - theta12(t) + theta22(t)) - 32.0*l1*m2*u11**2*cos(theta11(t) + theta12(t) - theta22(t)) + 24.0*l1*m2*u12**2*cos(theta11(t) - theta12(t) + theta22(t)) - 24.0*l1*m2*u12**2*cos(theta11(t) + theta12(t) - theta22(t)) - 8.0*l1*m2*u12**2*cos(3*theta11(t) - theta12(t) + theta22(t)) + 8.0*l1*m2*u12**2*cos(3*theta11(t) + theta12(t) - theta22(t)) - 128.0*l2*m1*u21*u22*cos(theta21(t)) + 8.0*l2*m2*u21**2*cos(-2*theta12(t) + theta21(t) + 2*theta22(t)) - 8.0*l2*m2*u21**2*cos(2*theta12(t) + theta21(t) - 2*theta22(t)) + 4.0*l2*m2*u21**2*cos(2*theta11(t) - 2*theta12(t) - theta21(t) + 2*theta22(t)) - 4.0*l2*m2*u21**2*cos(2*theta11(t) - 2*theta12(t) + theta21(t) + 2*theta22(t)) + 8.0*l2*m2*u21**2*cos(2*theta11(t) - theta12(t) - theta21(t) + theta22(t)) + 8.0*l2*m2*u21**2*cos(2*theta11(t) - theta12(t) + theta21(t) + theta22(t)) - 8.0*l2*m2*u21**2*cos(2*theta11(t) + theta12(t) - theta21(t) - theta22(t)) - 8.0*l2*m2*u21**2*cos(2*theta11(t) + theta12(t) + theta21(t) - theta22(t)) - 4.0*l2*m2*u21**2*cos(2*theta11(t) + 2*theta12(t) - theta21(t) - 2*theta22(t)) + 4.0*l2*m2*u21**2*cos(2*theta11(t) + 2*theta12(t) + theta21(t) - 2*theta22(t)) + 12.0*l2*m2*u21*u22*cos(2*theta11(t) - 3*theta21(t)) + 20.0*l2*m2*u21*u22*cos(2*theta11(t) - theta21(t)) + 20.0*l2*m2*u21*u22*cos(2*theta11(t) + theta21(t)) + 12.0*l2*m2*u21*u22*cos(2*theta11(t) + 3*theta21(t)) + 4.0*l2*m2*u21*u22*cos(-2*theta12(t) + theta21(t) + 2*theta22(t)) - 4.0*l2*m2*u21*u22*cos(-2*theta12(t) + 3*theta21(t) + 2*theta22(t)) + 4.0*l2*m2*u21*u22*cos(2*theta12(t) + theta21(t) - 2*theta22(t)) - 4.0*l2*m2*u21*u22*cos(2*theta12(t) + 3*theta21(t) - 2*theta22(t)) + 2.0*l2*m2*u21*u22*cos(2*theta11(t) - 2*theta12(t) - 3*theta21(t) + 2*theta22(t)) - 2.0*l2*m2*u21*u22*cos(2*theta11(t) - 2*theta12(t) - theta21(t) + 2*theta22(t)) - 2.0*l2*m2*u21*u22*cos(2*theta11(t) - 2*theta12(t) + theta21(t) + 2*theta22(t)) + 2.0*l2*m2*u21*u22*cos(2*theta11(t) - 2*theta12(t) + 3*theta21(t) + 2*theta22(t)) + 8.0*l2*m2*u21*u22*cos(2*theta11(t) - theta12(t) - 3*theta21(t) + theta22(t)) + 8.0*l2*m2*u21*u22*cos(2*theta11(t) - theta12(t) - theta21(t) + theta22(t)) - 8.0*l2*m2*u21*u22*cos(2*theta11(t) - theta12(t) + theta21(t) + theta22(t)) - 8.0*l2*m2*u21*u22*cos(2*theta11(t) - theta12(t) + 3*theta21(t) + theta22(t)) + 8.0*l2*m2*u21*u22*cos(2*theta11(t) + theta12(t) - 3*theta21(t) - theta22(t)) + 8.0*l2*m2*u21*u22*cos(2*theta11(t) + theta12(t) - theta21(t) - theta22(t)) - 8.0*l2*m2*u21*u22*cos(2*theta11(t) + theta12(t) + theta21(t) - theta22(t)) - 8.0*l2*m2*u21*u22*cos(2*theta11(t) + theta12(t) + 3*theta21(t) - theta22(t)) + 2.0*l2*m2*u21*u22*cos(2*theta11(t) + 2*theta12(t) - 3*theta21(t) - 2*theta22(t)) - 2.0*l2*m2*u21*u22*cos(2*theta11(t) + 2*theta12(t) - theta21(t) - 2*theta22(t)) - 2.0*l2*m2*u21*u22*cos(2*theta11(t) + 2*theta12(t) + theta21(t) - 2*theta22(t)) + 2.0*l2*m2*u21*u22*cos(2*theta11(t) + 2*theta12(t) + 3*theta21(t) - 2*theta22(t)) - 72.0*l2*m2*u21*u22*cos(theta21(t)) + 8.0*l2*m2*u21*u22*cos(3*theta21(t)) + 6.0*l2*m2*u22**2*cos(-2*theta12(t) + theta21(t) + 2*theta22(t)) - 2.0*l2*m2*u22**2*cos(-2*theta12(t) + 3*theta21(t) + 2*theta22(t)) - 6.0*l2*m2*u22**2*cos(2*theta12(t) + theta21(t) - 2*theta22(t)) + 2.0*l2*m2*u22**2*cos(2*theta12(t) + 3*theta21(t) - 2*theta22(t)) - 1.0*l2*m2*u22**2*cos(2*theta11(t) - 2*theta12(t) - 3*theta21(t) + 2*theta22(t)) + 3.0*l2*m2*u22**2*cos(2*theta11(t) - 2*theta12(t) - theta21(t) + 2*theta22(t)) - 3.0*l2*m2*u22**2*cos(2*theta11(t) - 2*theta12(t) + theta21(t) + 2*theta22(t)) + 1.0*l2*m2*u22**2*cos(2*theta11(t) - 2*theta12(t) + 3*theta21(t) + 2*theta22(t)) - 2.0*l2*m2*u22**2*cos(2*theta11(t) - theta12(t) - 3*theta21(t) + theta22(t)) + 2.0*l2*m2*u22**2*cos(2*theta11(t) - theta12(t) - theta21(t) + theta22(t)) + 2.0*l2*m2*u22**2*cos(2*theta11(t) - theta12(t) + theta21(t) + theta22(t)) - 2.0*l2*m2*u22**2*cos(2*theta11(t) - theta12(t) + 3*theta21(t) + theta22(t)) + 2.0*l2*m2*u22**2*cos(2*theta11(t) + theta12(t) - 3*theta21(t) - theta22(t)) - 2.0*l2*m2*u22**2*cos(2*theta11(t) + theta12(t) - theta21(t) - theta22(t)) - 2.0*l2*m2*u22**2*cos(2*theta11(t) + theta12(t) + theta21(t) - theta22(t)) + 2.0*l2*m2*u22**2*cos(2*theta11(t) + theta12(t) + 3*theta21(t) - theta22(t)) + 1.0*l2*m2*u22**2*cos(2*theta11(t) + 2*theta12(t) - 3*theta21(t) - 2*theta22(t)) - 3.0*l2*m2*u22**2*cos(2*theta11(t) + 2*theta12(t) - theta21(t) - 2*theta22(t)) + 3.0*l2*m2*u22**2*cos(2*theta11(t) + 2*theta12(t) + theta21(t) - 2*theta22(t)) - 1.0*l2*m2*u22**2*cos(2*theta11(t) + 2*theta12(t) + 3*theta21(t) - 2*theta22(t)))/(l2*(64*m1*sin(theta21(t)) + 6*m2*sin(2*theta11(t) - 3*theta21(t)) - 2*m2*sin(2*theta11(t) - theta21(t)) + 2*m2*sin(2*theta11(t) + theta21(t)) - 6*m2*sin(2*theta11(t) + 3*theta21(t)) - 6*m2*sin(-2*theta12(t) + theta21(t) + 2*theta22(t)) + 2*m2*sin(-2*theta12(t) + 3*theta21(t) + 2*theta22(t)) - 6*m2*sin(2*theta12(t) + theta21(t) - 2*theta22(t)) + 2*m2*sin(2*theta12(t) + 3*theta21(t) - 2*theta22(t)) + m2*sin(2*theta11(t) - 2*theta12(t) - 3*theta21(t) + 2*theta22(t)) - 3*m2*sin(2*theta11(t) - 2*theta12(t) - theta21(t) + 2*theta22(t)) + 3*m2*sin(2*theta11(t) - 2*theta12(t) + theta21(t) + 2*theta22(t)) - m2*sin(2*theta11(t) - 2*theta12(t) + 3*theta21(t) + 2*theta22(t)) + 4*m2*sin(2*theta11(t) - theta12(t) - 3*theta21(t) + theta22(t)) - 4*m2*sin(2*theta11(t) - theta12(t) - theta21(t) + theta22(t)) - 4*m2*sin(2*theta11(t) - theta12(t) + theta21(t) + theta22(t)) + 4*m2*sin(2*theta11(t) - theta12(t) + 3*theta21(t) + theta22(t)) + 4*m2*sin(2*theta11(t) + theta12(t) - 3*theta21(t) - theta22(t)) - 4*m2*sin(2*theta11(t) + theta12(t) - theta21(t) - theta22(t)) - 4*m2*sin(2*theta11(t) + theta12(t) + theta21(t) - theta22(t)) + 4*m2*sin(2*theta11(t) + theta12(t) + 3*theta21(t) - theta22(t)) + m2*sin(2*theta11(t) + 2*theta12(t) - 3*theta21(t) - 2*theta22(t)) - 3*m2*sin(2*theta11(t) + 2*theta12(t) - theta21(t) - 2*theta22(t)) + 3*m2*sin(2*theta11(t) + 2*theta12(t) + theta21(t) - 2*theta22(t)) - m2*sin(2*theta11(t) + 2*theta12(t) + 3*theta21(t) - 2*theta22(t)) + 44*m2*sin(theta21(t)) - 4*m2*sin(3*theta21(t))))
udot12 1.0*(-m2*(-sin(theta11(t))*cos(theta21(t)) + sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t)))*(1.0*g*m1*sin(theta11(t)) + 1.0*g*m2*sin(theta11(t)) - 0.5*l1*m1*u12**2*sin(2*theta11(t)) - 0.5*l1*m2*u12**2*sin(2*theta11(t)) + 1.0*l2*m2*u21**2*sin(theta11(t))*cos(theta21(t)) - 1.0*l2*m2*u21**2*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t)) + 2.0*l2*m2*u21*u22*sin(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l2*m2*u22**2*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t)))*sin(theta12(t) - theta22(t))*sin(theta21(t)) - m2*(-m1*cos(theta21(t)) - m2*sin(theta11(t))**2*cos(theta21(t)) + m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t)))*(-1.0*g*sin(theta21(t)) + 1.0*l1*u11**2*sin(theta11(t))*cos(theta12(t) - theta22(t))*cos(theta21(t)) - 1.0*l1*u11**2*sin(theta21(t))*cos(theta11(t)) + 2.0*l1*u11*u12*sin(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)) + 1.0*l1*u12**2*sin(theta11(t))*cos(theta12(t) - theta22(t))*cos(theta21(t)) + 0.5*l2*u22**2*sin(2*theta21(t)))*sin(theta12(t) - theta22(t)) + m2*(m2*(sin(theta11(t))*sin(theta21(t)) + cos(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)))**2*cos(theta12(t) - theta22(t)) + m2*(sin(theta11(t))*sin(theta21(t)) + cos(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)))*sin(theta12(t) - theta22(t))**2*cos(theta11(t))*cos(theta21(t)) - (m1 + m2)*cos(theta12(t) - theta22(t)))*(l1*u11**2*sin(theta12(t) - theta22(t))*sin(theta11(t)) - 2*l1*u11*u12*cos(theta12(t) - theta22(t))*cos(theta11(t)) + l1*u12**2*sin(theta12(t) - theta22(t))*sin(theta11(t)) - 2*l2*u21*u22*cos(theta21(t))) + (-m1 + m2*(sin(theta11(t))*sin(theta21(t)) + cos(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)))**2 + m2*sin(theta12(t) - theta22(t))**2*cos(theta11(t))**2 - m2)*(2*l1*m1*u11*u12*cos(theta11(t)) + 2*l1*m2*u11*u12*cos(theta11(t)) + l2*m2*u21**2*sin(theta12(t) - theta22(t))*sin(theta21(t)) + 2*l2*m2*u21*u22*cos(theta12(t) - theta22(t))*cos(theta21(t)) + l2*m2*u22**2*sin(theta12(t) - theta22(t))*sin(theta21(t))))/(l1*m1*(m1 + m2*sin(theta11(t))**2*sin(theta12(t))**2*sin(theta21(t))**2 - 2*m2*sin(theta11(t))**2*sin(theta12(t))*sin(theta21(t))**2*sin(theta22(t))*cos(theta12(t) - theta22(t)) + m2*sin(theta11(t))**2*sin(theta21(t))**2*sin(theta22(t))**2 - 2*m2*sin(theta11(t))**2*sin(theta21(t))**2 + m2*sin(theta11(t))**2 - 2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)) + m2*sin(theta21(t))**2)*sin(theta11(t)))
udot11 1.0*(-m2*(-sin(theta11(t))*cos(theta21(t)) + sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t)))*(2*l1*m1*u11*u12*cos(theta11(t)) + 2*l1*m2*u11*u12*cos(theta11(t)) + l2*m2*u21**2*sin(theta12(t) - theta22(t))*sin(theta21(t)) + 2*l2*m2*u21*u22*cos(theta12(t) - theta22(t))*cos(theta21(t)) + l2*m2*u22**2*sin(theta12(t) - theta22(t))*sin(theta21(t)))*sin(theta12(t) - theta22(t))*sin(theta21(t)) + m2*(-m1*cos(theta11(t)) + m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta21(t)) - m2*sin(theta21(t))**2*cos(theta11(t)))*(l1*u11**2*sin(theta12(t) - theta22(t))*sin(theta11(t)) - 2*l1*u11*u12*cos(theta12(t) - theta22(t))*cos(theta11(t)) + l1*u12**2*sin(theta12(t) - theta22(t))*sin(theta11(t)) - 2*l2*u21*u22*cos(theta21(t)))*sin(theta12(t) - theta22(t)) + m2*(m2*(sin(theta11(t))*sin(theta21(t)) + cos(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)))*cos(theta12(t) - theta22(t))**2 + m2*sin(theta12(t) - theta22(t))**2*cos(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)) - (m1 + m2)*(sin(theta11(t))*sin(theta21(t)) + cos(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t))))*(-1.0*g*sin(theta21(t)) + 1.0*l1*u11**2*sin(theta11(t))*cos(theta12(t) - theta22(t))*cos(theta21(t)) - 1.0*l1*u11**2*sin(theta21(t))*cos(theta11(t)) + 2.0*l1*u11*u12*sin(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)) + 1.0*l1*u12**2*sin(theta11(t))*cos(theta12(t) - theta22(t))*cos(theta21(t)) + 0.5*l2*u22**2*sin(2*theta21(t))) + (-m1 + m2*sin(theta12(t) - theta22(t))**2*cos(theta21(t))**2 + m2*cos(theta12(t) - theta22(t))**2 - m2)*(1.0*g*m1*sin(theta11(t)) + 1.0*g*m2*sin(theta11(t)) - 0.5*l1*m1*u12**2*sin(2*theta11(t)) - 0.5*l1*m2*u12**2*sin(2*theta11(t)) + 1.0*l2*m2*u21**2*sin(theta11(t))*cos(theta21(t)) - 1.0*l2*m2*u21**2*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t)) + 2.0*l2*m2*u21*u22*sin(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l2*m2*u22**2*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t))))/(l1*m1*(m1 + m2*sin(theta11(t))**2*sin(theta12(t))**2*sin(theta21(t))**2 - 2*m2*sin(theta11(t))**2*sin(theta12(t))*sin(theta21(t))**2*sin(theta22(t))*cos(theta12(t) - theta22(t)) + m2*sin(theta11(t))**2*sin(theta21(t))**2*sin(theta22(t))**2 - 2*m2*sin(theta11(t))**2*sin(theta21(t))**2 + m2*sin(theta11(t))**2 - 2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)) + m2*sin(theta21(t))**2))

Figure 92: Comparison of 2D and 3D double point mass pendulum given a small 2D excitation. As also discussed in Sec. 4.2.3 the description of the kinematics using spherical coordinates leads to the model flipping between nearly identical solutions that cause major steps in the azimuthal angle. While the position of the pendulum is not visibly affected this does lead very high azimuthal angular velocities. The kinetic energy associated with these leads to the dampening of the oscillation we can observe in this plot. Note that the signs of θ1j\theta_{1j} have been flipped where |θ2j|>π10|\theta_{2j}| > \frac{\pi}{10} to ease comparison of the two models.

Figure 93: Large excitation of double pendulum modelled with equations of motion derived using spherical coordinates. From left to right: front, side and top view. Note how the issues of spherical coordinates cause disturbances that quickly lead to chaotic out of plane motion.

Figure 94: Comparison of 2D and 3D double point mass pendulum given a large 2D exitation. Note how they are identical until for roughly the first five seconds and then begin to strongly deviate as the 3D pendulum begins to leave the plane of exitation. This is due to the issues discussed in Sec. 4.2.3 and illustrated in Fig. 38.

10.3.2 Projected Angles

""" EoM of 3d double pendulum using projected angles """
from sympy import symbols, Function, Eq
from sympy import sin, cos
from sympy import diff, simplify, expand, solve
from sympy import init_printing, pprint, latex

init_printing()

""" Define Symbols """
l1, l2, m1, m2 = symbols("l1 l2 m1 m2")
t = symbols("t")
g = symbols("g")
L, Pot, Kin = symbols("L Pot Kin")
theta11 = Function("theta11")(t)
theta12 = Function("theta12")(t)
theta21 = Function("theta21")(t)
theta22 = Function("theta22")(t)
u11, u12, udot11, udot12 = symbols("u11 u12 udot11 udot12")
u21, u22, udot21, udot22 = symbols("u21 u22 udot21 udot22")
x1, y1, z1, x2, y2, z2 = symbols("x1 y1 z1 x2 y2 z1")
q = [theta11, theta12, theta21, theta22]
substitutions = [
    (theta11.diff(t).diff(t), udot11),
    (theta12.diff(t).diff(t), udot12),
    (theta21.diff(t).diff(t), udot21),
    (theta22.diff(t).diff(t), udot22),
    (theta11.diff(t), u11),
    (theta12.diff(t), u12),
    (theta21.diff(t), u21),
    (theta22.diff(t), u22),
]

""" Define Kinematic Relations """
x1 = l1 * sin(theta11)
y1 = l1 * cos(theta11) * sin(theta12)
z1 = -l1 * cos(theta11) * cos(theta12)
x2 = x1 + l2 * sin(theta21)
y2 = y1 + l2 * cos(theta21) * sin(theta22)
z2 = z1 - l2 * cos(theta21) * cos(theta22)

""" Langrangian """
Pot = m1 * g * z1 + m2 * g * z2
Kin = 1 / 2 * m1 * (
    x1.diff(t) ** 2 + y1.diff(t) ** 2 + z1.diff(t) ** 2
) + 1 / 2 * m2 * (x2.diff(t) ** 2 + y2.diff(t) ** 2 + z2.diff(t) ** 2)
L = Kin - Pot

euleq = [Eq(0, L.diff(qi.diff(t)).diff(t) - L.diff(qi)).simplify() for qi in q]
euleq = [euleqi.subs(substitutions) for euleqi in euleq]

solvedeuleq = solve(
    euleq, [udot11, udot12, udot21, udot22], simplify=False, rational=False
)

for ddot, eqn in solvedeuleq.items():
    print(ddot, eqn.simplify())

udot11 (m2*(sin(theta11(t))*cos(theta12(t) - theta22(t))*cos(theta21(t)) - sin(theta21(t))*cos(theta11(t)))*(g*m1*sin(theta12(t)) + g*m2*sin(theta12(t)) - 2*l1*m1*u11*u12*sin(theta11(t)) - 2*l1*m2*u11*u12*sin(theta11(t)) + l2*m2*u21**2*sin(theta12(t) - theta22(t))*cos(theta21(t)) - 2*l2*m2*u21*u22*sin(theta21(t))*cos(theta12(t) - theta22(t)) + l2*m2*u22**2*sin(theta12(t) - theta22(t))*cos(theta21(t)))*sin(theta12(t) - theta22(t))*cos(theta21(t)) - m2*(-m1*sin(theta11(t)) - m2*sin(theta11(t))*cos(theta21(t))**2 + m2*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)))*(-g*sin(theta22(t)) + l1*u11**2*sin(theta12(t) - theta22(t))*cos(theta11(t)) + 2*l1*u11*u12*sin(theta11(t))*cos(theta12(t) - theta22(t)) + l1*u12**2*sin(theta12(t) - theta22(t))*cos(theta11(t)) + 2*l2*u21*u22*sin(theta21(t)))*sin(theta12(t) - theta22(t)) - m2*(m2*(sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t)) + cos(theta11(t))*cos(theta21(t)))*cos(theta12(t) - theta22(t))**2 + m2*sin(theta12(t) - theta22(t))**2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t)) + (-m1 - m2)*(sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t)) + cos(theta11(t))*cos(theta21(t))))*(g*sin(theta21(t))*cos(theta22(t)) - l1*u11**2*sin(theta11(t))*cos(theta21(t)) + l1*u11**2*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t)) - 2*l1*u11*u12*sin(theta12(t) - theta22(t))*sin(theta11(t))*sin(theta21(t)) + l1*u12**2*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t)) + l2*u22**2*sin(2*theta21(t))/2) + (-m1 + m2*sin(theta12(t) - theta22(t))**2*sin(theta21(t))**2 + m2*cos(theta12(t) - theta22(t))**2 - m2)*(g*m1*sin(theta11(t))*cos(theta12(t)) + g*m2*sin(theta11(t))*cos(theta12(t)) + l1*m1*u12**2*sin(2*theta11(t))/2 + l1*m2*u12**2*sin(2*theta11(t))/2 + l2*m2*u21**2*sin(theta11(t))*cos(theta12(t) - theta22(t))*cos(theta21(t)) - l2*m2*u21**2*sin(theta21(t))*cos(theta11(t)) + 2*l2*m2*u21*u22*sin(theta12(t) - theta22(t))*sin(theta11(t))*sin(theta21(t)) + l2*m2*u22**2*sin(theta11(t))*cos(theta12(t) - theta22(t))*cos(theta21(t))))/(l1*m1*(m1 - 2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)) - 2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta12(t) - theta22(t))*cos(theta11(t))**2*cos(theta21(t))**2 - m2*cos(theta11(t))**2*cos(theta12(t))**2*cos(theta21(t))**2 - m2*cos(theta11(t))**2*cos(theta21(t))**2*cos(theta22(t))**2 + m2*cos(theta11(t))**2 + m2*cos(theta21(t))**2))
udot12 (m2*(sin(theta11(t))*cos(theta12(t) - theta22(t))*cos(theta21(t)) - sin(theta21(t))*cos(theta11(t)))*(g*m1*sin(theta11(t))*cos(theta12(t)) + g*m2*sin(theta11(t))*cos(theta12(t)) + l1*m1*u12**2*sin(2*theta11(t))/2 + l1*m2*u12**2*sin(2*theta11(t))/2 + l2*m2*u21**2*sin(theta11(t))*cos(theta12(t) - theta22(t))*cos(theta21(t)) - l2*m2*u21**2*sin(theta21(t))*cos(theta11(t)) + 2*l2*m2*u21*u22*sin(theta12(t) - theta22(t))*sin(theta11(t))*sin(theta21(t)) + l2*m2*u22**2*sin(theta11(t))*cos(theta12(t) - theta22(t))*cos(theta21(t)))*sin(theta12(t) - theta22(t))*cos(theta21(t)) - m2*(-m1*sin(theta21(t)) + m2*sin(theta11(t))*cos(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)) - m2*sin(theta21(t))*cos(theta11(t))**2)*(g*sin(theta21(t))*cos(theta22(t)) - l1*u11**2*sin(theta11(t))*cos(theta21(t)) + l1*u11**2*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t)) - 2*l1*u11*u12*sin(theta12(t) - theta22(t))*sin(theta11(t))*sin(theta21(t)) + l1*u12**2*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t)) + l2*u22**2*sin(2*theta21(t))/2)*sin(theta12(t) - theta22(t)) + m2*(m2*(sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t)) + cos(theta11(t))*cos(theta21(t)))**2*cos(theta12(t) - theta22(t)) + m2*(sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t)) + cos(theta11(t))*cos(theta21(t)))*sin(theta12(t) - theta22(t))**2*sin(theta11(t))*sin(theta21(t)) + (-m1 - m2)*cos(theta12(t) - theta22(t)))*(-g*sin(theta22(t)) + l1*u11**2*sin(theta12(t) - theta22(t))*cos(theta11(t)) + 2*l1*u11*u12*sin(theta11(t))*cos(theta12(t) - theta22(t)) + l1*u12**2*sin(theta12(t) - theta22(t))*cos(theta11(t)) + 2*l2*u21*u22*sin(theta21(t))) + (-m1 + m2*(sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t)) + cos(theta11(t))*cos(theta21(t)))**2 + m2*sin(theta12(t) - theta22(t))**2*sin(theta11(t))**2 - m2)*(g*m1*sin(theta12(t)) + g*m2*sin(theta12(t)) - 2*l1*m1*u11*u12*sin(theta11(t)) - 2*l1*m2*u11*u12*sin(theta11(t)) + l2*m2*u21**2*sin(theta12(t) - theta22(t))*cos(theta21(t)) - 2*l2*m2*u21*u22*sin(theta21(t))*cos(theta12(t) - theta22(t)) + l2*m2*u22**2*sin(theta12(t) - theta22(t))*cos(theta21(t))))/(l1*m1*(m1 - 2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)) - 2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta12(t) - theta22(t))*cos(theta11(t))**2*cos(theta21(t))**2 - m2*cos(theta11(t))**2*cos(theta12(t))**2*cos(theta21(t))**2 - m2*cos(theta11(t))**2*cos(theta21(t))**2*cos(theta22(t))**2 + m2*cos(theta11(t))**2 + m2*cos(theta21(t))**2)*cos(theta11(t)))
udot21 (m2*(m1 + m2)*(-sin(theta11(t))*cos(theta21(t)) + sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t)))*(-g*sin(theta22(t)) + l1*u11**2*sin(theta12(t) - theta22(t))*cos(theta11(t)) + 2*l1*u11*u12*sin(theta11(t))*cos(theta12(t) - theta22(t)) + l1*u12**2*sin(theta12(t) - theta22(t))*cos(theta11(t)) + 2*l2*u21*u22*sin(theta21(t)))*sin(theta12(t) - theta22(t))*cos(theta11(t)) + (m1 + m2)*(-m1 + m2*sin(theta12(t) - theta22(t))**2*sin(theta11(t))**2 + m2*cos(theta12(t) - theta22(t))**2 - m2)*(g*sin(theta21(t))*cos(theta22(t)) - l1*u11**2*sin(theta11(t))*cos(theta21(t)) + l1*u11**2*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t)) - 2*l1*u11*u12*sin(theta12(t) - theta22(t))*sin(theta11(t))*sin(theta21(t)) + l1*u12**2*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t)) + l2*u22**2*sin(2*theta21(t))/2) - (-m1*sin(theta21(t)) + m2*sin(theta11(t))*cos(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)) - m2*sin(theta21(t))*cos(theta11(t))**2)*(g*m1*sin(theta12(t)) + g*m2*sin(theta12(t)) - 2*l1*m1*u11*u12*sin(theta11(t)) - 2*l1*m2*u11*u12*sin(theta11(t)) + l2*m2*u21**2*sin(theta12(t) - theta22(t))*cos(theta21(t)) - 2*l2*m2*u21*u22*sin(theta21(t))*cos(theta12(t) - theta22(t)) + l2*m2*u22**2*sin(theta12(t) - theta22(t))*cos(theta21(t)))*sin(theta12(t) - theta22(t)) - (m2*(sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t)) + cos(theta11(t))*cos(theta21(t)))*cos(theta12(t) - theta22(t))**2 + m2*sin(theta12(t) - theta22(t))**2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t)) + (-m1 - m2)*(sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t)) + cos(theta11(t))*cos(theta21(t))))*(g*m1*sin(theta11(t))*cos(theta12(t)) + g*m2*sin(theta11(t))*cos(theta12(t)) + l1*m1*u12**2*sin(2*theta11(t))/2 + l1*m2*u12**2*sin(2*theta11(t))/2 + l2*m2*u21**2*sin(theta11(t))*cos(theta12(t) - theta22(t))*cos(theta21(t)) - l2*m2*u21**2*sin(theta21(t))*cos(theta11(t)) + 2*l2*m2*u21*u22*sin(theta12(t) - theta22(t))*sin(theta11(t))*sin(theta21(t)) + l2*m2*u22**2*sin(theta11(t))*cos(theta12(t) - theta22(t))*cos(theta21(t))))/(l2*m1*(m1 - 2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)) - 2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta12(t) - theta22(t))*cos(theta11(t))**2*cos(theta21(t))**2 - m2*cos(theta11(t))**2*cos(theta12(t))**2*cos(theta21(t))**2 - m2*cos(theta11(t))**2*cos(theta21(t))**2*cos(theta22(t))**2 + m2*cos(theta11(t))**2 + m2*cos(theta21(t))**2))
udot22 (-m2*(m1 + m2)*(-sin(theta11(t))*cos(theta21(t)) + sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t)))*(g*sin(theta21(t))*cos(theta22(t)) - l1*u11**2*sin(theta11(t))*cos(theta21(t)) + l1*u11**2*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t)) - 2*l1*u11*u12*sin(theta12(t) - theta22(t))*sin(theta11(t))*sin(theta21(t)) + l1*u12**2*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t)) + l2*u22**2*sin(2*theta21(t))/2)*sin(theta12(t) - theta22(t))*cos(theta11(t)) + (-m1 - m2)*(-m1 + m2*(sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t)) + cos(theta11(t))*cos(theta21(t)))**2 + m2*sin(theta12(t) - theta22(t))**2*sin(theta21(t))**2 - m2)*(-g*sin(theta22(t)) + l1*u11**2*sin(theta12(t) - theta22(t))*cos(theta11(t)) + 2*l1*u11*u12*sin(theta11(t))*cos(theta12(t) - theta22(t)) + l1*u12**2*sin(theta12(t) - theta22(t))*cos(theta11(t)) + 2*l2*u21*u22*sin(theta21(t))) + (-m1*sin(theta11(t)) - m2*sin(theta11(t))*cos(theta21(t))**2 + m2*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)))*(g*m1*sin(theta11(t))*cos(theta12(t)) + g*m2*sin(theta11(t))*cos(theta12(t)) + l1*m1*u12**2*sin(2*theta11(t))/2 + l1*m2*u12**2*sin(2*theta11(t))/2 + l2*m2*u21**2*sin(theta11(t))*cos(theta12(t) - theta22(t))*cos(theta21(t)) - l2*m2*u21**2*sin(theta21(t))*cos(theta11(t)) + 2*l2*m2*u21*u22*sin(theta12(t) - theta22(t))*sin(theta11(t))*sin(theta21(t)) + l2*m2*u22**2*sin(theta11(t))*cos(theta12(t) - theta22(t))*cos(theta21(t)))*sin(theta12(t) - theta22(t)) - (m2*(sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t)) + cos(theta11(t))*cos(theta21(t)))**2*cos(theta12(t) - theta22(t)) + m2*(sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t)) + cos(theta11(t))*cos(theta21(t)))*sin(theta12(t) - theta22(t))**2*sin(theta11(t))*sin(theta21(t)) + (-m1 - m2)*cos(theta12(t) - theta22(t)))*(g*m1*sin(theta12(t)) + g*m2*sin(theta12(t)) - 2*l1*m1*u11*u12*sin(theta11(t)) - 2*l1*m2*u11*u12*sin(theta11(t)) + l2*m2*u21**2*sin(theta12(t) - theta22(t))*cos(theta21(t)) - 2*l2*m2*u21*u22*sin(theta21(t))*cos(theta12(t) - theta22(t)) + l2*m2*u22**2*sin(theta12(t) - theta22(t))*cos(theta21(t))))/(l2*m1*(m1 - 2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t) - theta22(t))*cos(theta11(t))*cos(theta21(t)) - 2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta12(t) - theta22(t))*cos(theta11(t))**2*cos(theta21(t))**2 - m2*cos(theta11(t))**2*cos(theta12(t))**2*cos(theta21(t))**2 - m2*cos(theta11(t))**2*cos(theta21(t))**2*cos(theta22(t))**2 + m2*cos(theta11(t))**2 + m2*cos(theta21(t))**2)*cos(theta21(t)))

Figure 95: 3D double pendulum using projected angles under small 2D exitation. Note how we no longer have the jumps in the second angle observed in the simulations using spherical coordinates. The motion remains entirely in the XZ-plane. Note that this is no longer the case when the planar exitation lies in a plane outside of the XZ- or YZ-plane (see Fig. 59).

Figure 96: Comparison of 2D and 3D model (projected angles) of double pendulum with small planar exitation. Note the stark decrease in difference and lack of dampening when compared with the spherical coordinate model.

10.4 3D Distributed Mass Pendulum

""" EoM of 3D double pendulum with the second mass distributed """
from sympy import symbols, Function, Eq, Matrix
from sympy import sin, cos
from sympy import diff, simplify, expand, solve, factor
from sympy import init_printing, pprint, latex

init_printing()

""" Define Symbols """
l1, l2, m1, m2 = symbols("l1 l2 m1 m2")
Ixx, Iyy, Izz = symbols("Ixx, Iyy, Izz")
t = symbols("t")
g = symbols("g")
L, Pot, Kin = symbols("L Pot Kin")
theta11 = Function("theta11")(t)
theta12 = Function("theta12")(t)
theta21 = Function("theta21")(t)
theta22 = Function("theta22")(t)
theta23 = Function("theta23")(t)
u11, u12, udot11, udot12 = symbols("u11 u12 udot11 udot12")
u21, u22, u23, udot21, udot22, udot23 = symbols("u21 u22 u23 udot21 udot22 udot23")
x1, y1, z1, x2, y2, z2 = symbols("x1 y1 z1 x2 y2 z1")
q = [theta11, theta12, theta21, theta22, theta23]
substitutions = [
    (theta11.diff(t).diff(t), udot11),
    (theta12.diff(t).diff(t), udot12),
    (theta21.diff(t).diff(t), udot21),
    (theta22.diff(t).diff(t), udot22),
    (theta23.diff(t).diff(t), udot23),
    (theta11.diff(t), u11),
    (theta12.diff(t), u12),
    (theta21.diff(t), u21),
    (theta22.diff(t), u22),
    (theta23.diff(t), u23),
]

""" Define Kinematic Relations """
x1 = l1 * sin(theta11)
y1 = l1 * cos(theta11) * sin(theta12)
z1 = -l1 * cos(theta11) * cos(theta12)
x2 = x1 + l2 * sin(theta21)
y2 = y1 + l2 * cos(theta21) * sin(theta22)
z2 = z1 - l2 * cos(theta21) * cos(theta22)

""" Equations for omega from cog-rotations.py """
omega_x = -theta21.diff(t) * (
    -sin(theta21) * sin(theta22) * cos(theta23) - sin(theta23) * cos(theta21)
) + theta22.diff(t) * (
    -sin(theta21) * sin(theta22) * sin(theta23) + cos(theta21) * cos(theta23)
)
omega_y = -theta21.diff(t) * cos(theta22) * cos(theta23) + theta22.diff(t) * sin(
    theta23
) * cos(theta22)
omega_z = (
    -theta21.diff(t)
    * (-sin(theta21) * sin(theta23) + sin(theta22) * cos(theta21) * cos(theta23))
    + theta22.diff(t)
    * (sin(theta21) * cos(theta23) + sin(theta22) * sin(theta23) * cos(theta21))
    + theta23.diff(t)
)

""" Langrangian """
Pot = m1 * g * z1 + m2 * g * z2
Kin = (
    1 / 2 * m1 * (x1.diff(t) ** 2 + y1.diff(t) ** 2 + z1.diff(t) ** 2)
    + 1 / 2 * m2 * (x2.diff(t) ** 2 + y2.diff(t) ** 2 + z2.diff(t) ** 2)
    + 1 / 2 * Ixx * omega_x ** 2
    + 1 / 2 * Iyy * omega_y ** 2
    + 1 / 2 * Izz * omega_z ** 2
)
L = Kin - Pot

euleq = [Eq(0, L.diff(qi.diff(t)).diff(t) - L.diff(qi)) for qi in q]
euleq = [euleqi.subs(substitutions).expand() for euleqi in euleq]

solvedeuleq = solve(
    euleq, [udot11, udot12, udot21, udot22, udot23], simplify=True, rational=False
)

for ddot, eqn in solvedeuleq.items():
    print(ddot, eqn)

udot11 (2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2))*(1.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))**2 - 1.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))*cos(theta22(t))*cos(theta23(t))**2 - 1.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))*cos(theta23(t))**2 - 2.0*Ixx*u21**2*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*u21**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) - 2.0*Ixx*u21**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*u21**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t))**2 + 1.0*Ixx*u21**2*sin(theta22(t))*cos(theta21(t))**2*cos(theta23(t))**2 + 2.0*Ixx*u21*u22*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) + 2.0*Ixx*u21*u22*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Ixx*u21*u22*sin(theta21(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Ixx*u21*u22*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Ixx*u21*u23*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 - 1.0*Ixx*u21*u23*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 4.0*Ixx*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*u21*u23*sin(theta23(t))**2*cos(theta21(t))**2 + 1.0*Ixx*u21*u23*cos(theta21(t))**2*cos(theta23(t))**2 + 1.0*Ixx*u22**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta22(t)) - 1.0*Ixx*u22**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 2.0*Ixx*u22*u23*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 2.0*Ixx*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Ixx*u22*u23*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Ixx*u22*u23*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*u21**2*sin(theta22(t))*cos(theta22(t))*cos(theta23(t))**2 + 1.0*Iyy*u21*u23*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Iyy*u21*u23*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Iyy*u22**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta22(t)) + 2.0*Iyy*u22*u23*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*u21**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))**2 + 1.0*Izz*u21**2*sin(theta21(t))**2*sin(theta22(t))*cos(theta23(t))**2 + 2.0*Izz*u21**2*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u21**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 2.0*Izz*u21**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u21**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Izz*u21**2*sin(theta22(t))*cos(theta21(t))**2*cos(theta22(t))*cos(theta23(t))**2 - 1.0*Izz*u21**2*sin(theta22(t))*cos(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*u21*u22*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) - 2.0*Izz*u21*u22*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Izz*u21*u22*sin(theta21(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Izz*u21*u22*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 1.0*Izz*u21*u23*sin(theta21(t))**2*sin(theta23(t))**2 + 1.0*Izz*u21*u23*sin(theta21(t))**2*cos(theta23(t))**2 + 4.0*Izz*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t)) + 1.0*Izz*u21*u23*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Izz*u21*u23*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 + 1.0*Izz*u21*u23*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 1.0*Izz*u21*u23*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u22**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 1.0*Izz*u22**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t))**2*cos(theta22(t)) - 2.0*Izz*u22*u23*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 2.0*Izz*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Izz*u22*u23*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Izz*u22*u23*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 1.0*Izz*u23**2*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*u23**2*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)) + g*l2*m2*sin(theta22(t))*cos(theta21(t)) - 1.0*l1*l2*m2*u11**2*sin(theta12(t))*cos(theta11(t))*cos(theta21(t))*cos(theta22(t)) + 1.0*l1*l2*m2*u11**2*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))*cos(theta21(t)) - 2.0*l1*l2*m2*u11*u12*sin(theta11(t))*sin(theta12(t))*sin(theta22(t))*cos(theta21(t)) - 2.0*l1*l2*m2*u11*u12*sin(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u12**2*sin(theta12(t))*cos(theta11(t))*cos(theta21(t))*cos(theta22(t)) + 1.0*l1*l2*m2*u12**2*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))*cos(theta21(t)) - 2.0*l2**2*m2*u21*u22*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t)) - 2.0*l2**2*m2*u21*u22*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))**2)/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2)) + (-2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t))) + 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2))*(-1.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*u21**2*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*u21**2*sin(theta21(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Ixx*u21**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 2.0*Ixx*u21*u22*sin(theta21(t))**2*sin(theta22(t))*cos(theta22(t))*cos(theta23(t))**2 + 2.0*Ixx*u21*u22*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) - 2.0*Ixx*u21*u23*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) - 2.0*Ixx*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Ixx*u21*u23*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Ixx*u21*u23*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2.0*Ixx*u22**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta22(t))*cos(theta23(t)) - 1.0*Ixx*u22**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) - 1.0*Ixx*u22**2*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*u22**2*sin(theta21(t))*sin(theta23(t))**2*cos(theta21(t))*cos(theta22(t)) + 1.0*Ixx*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t))**2 + 1.0*Ixx*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Ixx*u22**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Ixx*u22*u23*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 - 1.0*Ixx*u22*u23*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 4.0*Ixx*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*u22*u23*sin(theta23(t))**2*cos(theta21(t))**2 + 1.0*Ixx*u22*u23*cos(theta21(t))**2*cos(theta23(t))**2 - 2.0*Iyy*u21*u22*sin(theta22(t))*cos(theta22(t))*cos(theta23(t))**2 - 2.0*Iyy*u21*u23*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) + 2.0*Iyy*u22**2*sin(theta22(t))*sin(theta23(t))*cos(theta22(t))*cos(theta23(t)) + 1.0*Iyy*u22*u23*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Iyy*u22*u23*cos(theta22(t))**2*cos(theta23(t))**2 + 1.0*Izz*u21**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*u21**2*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*u21**2*sin(theta21(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Izz*u21**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2.0*Izz*u21*u22*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 2.0*Izz*u21*u22*sin(theta22(t))*cos(theta21(t))**2*cos(theta22(t))*cos(theta23(t))**2 + 2.0*Izz*u21*u23*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) + 2.0*Izz*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Izz*u21*u23*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Izz*u21*u23*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Izz*u22**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) + 1.0*Izz*u22**2*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*u22**2*sin(theta21(t))*sin(theta23(t))**2*cos(theta21(t))*cos(theta22(t)) - 1.0*Izz*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t))**2 - 1.0*Izz*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Izz*u22**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta22(t))*cos(theta23(t)) - 1.0*Izz*u22**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 1.0*Izz*u22*u23*sin(theta21(t))**2*sin(theta23(t))**2 + 1.0*Izz*u22*u23*sin(theta21(t))**2*cos(theta23(t))**2 + 4.0*Izz*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t)) + 1.0*Izz*u22*u23*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Izz*u22*u23*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Izz*u22*u23*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) - 1.0*Izz*u22*u23*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u23**2*sin(theta21(t))*cos(theta23(t)) + 1.0*Izz*u23**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)) + g*l2*m2*sin(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u11**2*sin(theta11(t))*cos(theta21(t)) + 1.0*l1*l2*m2*u11**2*sin(theta12(t))*sin(theta21(t))*sin(theta22(t))*cos(theta11(t)) + 1.0*l1*l2*m2*u11**2*sin(theta21(t))*cos(theta11(t))*cos(theta12(t))*cos(theta22(t)) - 2.0*l1*l2*m2*u11*u12*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*cos(theta22(t)) + 2.0*l1*l2*m2*u11*u12*sin(theta11(t))*sin(theta21(t))*sin(theta22(t))*cos(theta12(t)) + 1.0*l1*l2*m2*u12**2*sin(theta12(t))*sin(theta21(t))*sin(theta22(t))*cos(theta11(t)) + 1.0*l1*l2*m2*u12**2*sin(theta21(t))*cos(theta11(t))*cos(theta12(t))*cos(theta22(t)) + 1.0*l2**2*m2*u21**2*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t)) + 1.0*l2**2*m2*u21**2*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))**2 - 1.0*l2**2*m2*u21**2*sin(theta21(t))*cos(theta21(t)) + 1.0*l2**2*m2*u22**2*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t)) + 1.0*l2**2*m2*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))**2)/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2)) + (-2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t))) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))))*(g*l1*m1*sin(theta12(t))*cos(theta11(t)) + g*l1*m2*sin(theta12(t))*cos(theta11(t)) - 2.0*l1**2*m1*u11*u12*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) - 2.0*l1**2*m1*u11*u12*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 - 2.0*l1**2*m2*u11*u12*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) - 2.0*l1**2*m2*u11*u12*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 + 1.0*l1*l2*m2*u21**2*sin(theta12(t))*cos(theta11(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u21**2*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))*cos(theta21(t)) - 2.0*l1*l2*m2*u21*u22*sin(theta12(t))*sin(theta21(t))*sin(theta22(t))*cos(theta11(t)) - 2.0*l1*l2*m2*u21*u22*sin(theta21(t))*cos(theta11(t))*cos(theta12(t))*cos(theta22(t)) + 1.0*l1*l2*m2*u22**2*sin(theta12(t))*cos(theta11(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u22**2*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))*cos(theta21(t)))/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2)) + ((-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t))) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))))*(-2.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) - 2.0*Ixx*u21**2*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Ixx*u21**2*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Ixx*u21**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 2.0*Ixx*u21*u22*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 - 2.0*Ixx*u21*u22*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 8.0*Ixx*u21*u22*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 2.0*Ixx*u21*u22*sin(theta23(t))**2*cos(theta21(t))**2 + 2.0*Ixx*u21*u22*cos(theta21(t))**2*cos(theta23(t))**2 + 2.0*Ixx*u22**2*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 2.0*Ixx*u22**2*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Ixx*u22**2*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Ixx*u22**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2.0*Iyy*u21**2*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) + 2.0*Iyy*u21*u22*sin(theta23(t))**2*cos(theta22(t))**2 - 2.0*Iyy*u21*u22*cos(theta22(t))**2*cos(theta23(t))**2 + 2.0*Iyy*u22**2*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) + 2.0*Izz*u21**2*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) + 2.0*Izz*u21**2*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Izz*u21**2*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 2*Izz*u21**2*sin(theta21(t))*sin(theta22(t))*cos(theta23(t)) - 2.0*Izz*u21**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2*Izz*u21**2*sin(theta23(t))*cos(theta21(t)) - 2.0*Izz*u21*u22*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*u21*u22*sin(theta21(t))**2*cos(theta23(t))**2 + 8.0*Izz*u21*u22*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) + 2*Izz*u21*u22*sin(theta21(t))*sin(theta22(t))*sin(theta23(t)) + 2.0*Izz*u21*u22*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 2.0*Izz*u21*u22*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 + 2*Izz*u21*u22*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) - 2*Izz*u21*u22*cos(theta21(t))*cos(theta23(t)) - 2.0*Izz*u22**2*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 2.0*Izz*u22**2*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Izz*u22**2*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Izz*u22**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2*Izz*u22**2*sin(theta23(t))*cos(theta21(t))*cos(theta22(t)))/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2)) + (-2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2 - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t))) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t))) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2))*(g*l1*m1*sin(theta11(t))*cos(theta12(t)) + g*l1*m2*sin(theta11(t))*cos(theta12(t)) + 1.0*l1**2*m1*u11**2*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) + 1.0*l1**2*m1*u11**2*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 - 1.0*l1**2*m1*u11**2*sin(theta11(t))*cos(theta11(t)) + 1.0*l1**2*m1*u12**2*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) + 1.0*l1**2*m1*u12**2*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 + 1.0*l1**2*m2*u11**2*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) + 1.0*l1**2*m2*u11**2*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 - 1.0*l1**2*m2*u11**2*sin(theta11(t))*cos(theta11(t)) + 1.0*l1**2*m2*u12**2*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) + 1.0*l1**2*m2*u12**2*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 + 1.0*l1*l2*m2*u21**2*sin(theta11(t))*sin(theta12(t))*sin(theta22(t))*cos(theta21(t)) + 1.0*l1*l2*m2*u21**2*sin(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u21**2*sin(theta21(t))*cos(theta11(t)) + 2.0*l1*l2*m2*u21*u22*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*cos(theta22(t)) - 2.0*l1*l2*m2*u21*u22*sin(theta11(t))*sin(theta21(t))*sin(theta22(t))*cos(theta12(t)) + 1.0*l1*l2*m2*u22**2*sin(theta11(t))*sin(theta12(t))*sin(theta22(t))*cos(theta21(t)) + 1.0*l1*l2*m2*u22**2*sin(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2))
udot12 (2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))) - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2))*(-1.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*u21**2*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*u21**2*sin(theta21(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Ixx*u21**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 2.0*Ixx*u21*u22*sin(theta21(t))**2*sin(theta22(t))*cos(theta22(t))*cos(theta23(t))**2 + 2.0*Ixx*u21*u22*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) - 2.0*Ixx*u21*u23*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) - 2.0*Ixx*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Ixx*u21*u23*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Ixx*u21*u23*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2.0*Ixx*u22**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta22(t))*cos(theta23(t)) - 1.0*Ixx*u22**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) - 1.0*Ixx*u22**2*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*u22**2*sin(theta21(t))*sin(theta23(t))**2*cos(theta21(t))*cos(theta22(t)) + 1.0*Ixx*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t))**2 + 1.0*Ixx*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Ixx*u22**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Ixx*u22*u23*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 - 1.0*Ixx*u22*u23*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 4.0*Ixx*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*u22*u23*sin(theta23(t))**2*cos(theta21(t))**2 + 1.0*Ixx*u22*u23*cos(theta21(t))**2*cos(theta23(t))**2 - 2.0*Iyy*u21*u22*sin(theta22(t))*cos(theta22(t))*cos(theta23(t))**2 - 2.0*Iyy*u21*u23*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) + 2.0*Iyy*u22**2*sin(theta22(t))*sin(theta23(t))*cos(theta22(t))*cos(theta23(t)) + 1.0*Iyy*u22*u23*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Iyy*u22*u23*cos(theta22(t))**2*cos(theta23(t))**2 + 1.0*Izz*u21**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*u21**2*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*u21**2*sin(theta21(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Izz*u21**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2.0*Izz*u21*u22*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 2.0*Izz*u21*u22*sin(theta22(t))*cos(theta21(t))**2*cos(theta22(t))*cos(theta23(t))**2 + 2.0*Izz*u21*u23*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) + 2.0*Izz*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Izz*u21*u23*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Izz*u21*u23*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Izz*u22**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) + 1.0*Izz*u22**2*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*u22**2*sin(theta21(t))*sin(theta23(t))**2*cos(theta21(t))*cos(theta22(t)) - 1.0*Izz*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t))**2 - 1.0*Izz*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Izz*u22**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta22(t))*cos(theta23(t)) - 1.0*Izz*u22**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 1.0*Izz*u22*u23*sin(theta21(t))**2*sin(theta23(t))**2 + 1.0*Izz*u22*u23*sin(theta21(t))**2*cos(theta23(t))**2 + 4.0*Izz*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t)) + 1.0*Izz*u22*u23*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Izz*u22*u23*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Izz*u22*u23*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) - 1.0*Izz*u22*u23*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u23**2*sin(theta21(t))*cos(theta23(t)) + 1.0*Izz*u23**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)) + g*l2*m2*sin(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u11**2*sin(theta11(t))*cos(theta21(t)) + 1.0*l1*l2*m2*u11**2*sin(theta12(t))*sin(theta21(t))*sin(theta22(t))*cos(theta11(t)) + 1.0*l1*l2*m2*u11**2*sin(theta21(t))*cos(theta11(t))*cos(theta12(t))*cos(theta22(t)) - 2.0*l1*l2*m2*u11*u12*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*cos(theta22(t)) + 2.0*l1*l2*m2*u11*u12*sin(theta11(t))*sin(theta21(t))*sin(theta22(t))*cos(theta12(t)) + 1.0*l1*l2*m2*u12**2*sin(theta12(t))*sin(theta21(t))*sin(theta22(t))*cos(theta11(t)) + 1.0*l1*l2*m2*u12**2*sin(theta21(t))*cos(theta11(t))*cos(theta12(t))*cos(theta22(t)) + 1.0*l2**2*m2*u21**2*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t)) + 1.0*l2**2*m2*u21**2*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))**2 - 1.0*l2**2*m2*u21**2*sin(theta21(t))*cos(theta21(t)) + 1.0*l2**2*m2*u22**2*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t)) + 1.0*l2**2*m2*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))**2)/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2)) + (-2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2))*(1.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))**2 - 1.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))*cos(theta22(t))*cos(theta23(t))**2 - 1.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))*cos(theta23(t))**2 - 2.0*Ixx*u21**2*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*u21**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) - 2.0*Ixx*u21**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*u21**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t))**2 + 1.0*Ixx*u21**2*sin(theta22(t))*cos(theta21(t))**2*cos(theta23(t))**2 + 2.0*Ixx*u21*u22*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) + 2.0*Ixx*u21*u22*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Ixx*u21*u22*sin(theta21(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Ixx*u21*u22*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Ixx*u21*u23*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 - 1.0*Ixx*u21*u23*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 4.0*Ixx*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*u21*u23*sin(theta23(t))**2*cos(theta21(t))**2 + 1.0*Ixx*u21*u23*cos(theta21(t))**2*cos(theta23(t))**2 + 1.0*Ixx*u22**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta22(t)) - 1.0*Ixx*u22**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 2.0*Ixx*u22*u23*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 2.0*Ixx*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Ixx*u22*u23*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Ixx*u22*u23*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*u21**2*sin(theta22(t))*cos(theta22(t))*cos(theta23(t))**2 + 1.0*Iyy*u21*u23*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Iyy*u21*u23*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Iyy*u22**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta22(t)) + 2.0*Iyy*u22*u23*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*u21**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))**2 + 1.0*Izz*u21**2*sin(theta21(t))**2*sin(theta22(t))*cos(theta23(t))**2 + 2.0*Izz*u21**2*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u21**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 2.0*Izz*u21**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u21**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Izz*u21**2*sin(theta22(t))*cos(theta21(t))**2*cos(theta22(t))*cos(theta23(t))**2 - 1.0*Izz*u21**2*sin(theta22(t))*cos(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*u21*u22*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) - 2.0*Izz*u21*u22*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Izz*u21*u22*sin(theta21(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Izz*u21*u22*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 1.0*Izz*u21*u23*sin(theta21(t))**2*sin(theta23(t))**2 + 1.0*Izz*u21*u23*sin(theta21(t))**2*cos(theta23(t))**2 + 4.0*Izz*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t)) + 1.0*Izz*u21*u23*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Izz*u21*u23*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 + 1.0*Izz*u21*u23*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 1.0*Izz*u21*u23*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u22**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 1.0*Izz*u22**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t))**2*cos(theta22(t)) - 2.0*Izz*u22*u23*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 2.0*Izz*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Izz*u22*u23*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Izz*u22*u23*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 1.0*Izz*u23**2*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*u23**2*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)) + g*l2*m2*sin(theta22(t))*cos(theta21(t)) - 1.0*l1*l2*m2*u11**2*sin(theta12(t))*cos(theta11(t))*cos(theta21(t))*cos(theta22(t)) + 1.0*l1*l2*m2*u11**2*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))*cos(theta21(t)) - 2.0*l1*l2*m2*u11*u12*sin(theta11(t))*sin(theta12(t))*sin(theta22(t))*cos(theta21(t)) - 2.0*l1*l2*m2*u11*u12*sin(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u12**2*sin(theta12(t))*cos(theta11(t))*cos(theta21(t))*cos(theta22(t)) + 1.0*l1*l2*m2*u12**2*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))*cos(theta21(t)) - 2.0*l2**2*m2*u21*u22*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t)) - 2.0*l2**2*m2*u21*u22*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))**2)/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2)) + (-(-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2))*(-2.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) - 2.0*Ixx*u21**2*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Ixx*u21**2*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Ixx*u21**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 2.0*Ixx*u21*u22*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 - 2.0*Ixx*u21*u22*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 8.0*Ixx*u21*u22*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 2.0*Ixx*u21*u22*sin(theta23(t))**2*cos(theta21(t))**2 + 2.0*Ixx*u21*u22*cos(theta21(t))**2*cos(theta23(t))**2 + 2.0*Ixx*u22**2*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 2.0*Ixx*u22**2*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Ixx*u22**2*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Ixx*u22**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2.0*Iyy*u21**2*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) + 2.0*Iyy*u21*u22*sin(theta23(t))**2*cos(theta22(t))**2 - 2.0*Iyy*u21*u22*cos(theta22(t))**2*cos(theta23(t))**2 + 2.0*Iyy*u22**2*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) + 2.0*Izz*u21**2*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) + 2.0*Izz*u21**2*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Izz*u21**2*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 2*Izz*u21**2*sin(theta21(t))*sin(theta22(t))*cos(theta23(t)) - 2.0*Izz*u21**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2*Izz*u21**2*sin(theta23(t))*cos(theta21(t)) - 2.0*Izz*u21*u22*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*u21*u22*sin(theta21(t))**2*cos(theta23(t))**2 + 8.0*Izz*u21*u22*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) + 2*Izz*u21*u22*sin(theta21(t))*sin(theta22(t))*sin(theta23(t)) + 2.0*Izz*u21*u22*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 2.0*Izz*u21*u22*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 + 2*Izz*u21*u22*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) - 2*Izz*u21*u22*cos(theta21(t))*cos(theta23(t)) - 2.0*Izz*u22**2*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 2.0*Izz*u22**2*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Izz*u22**2*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Izz*u22**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2*Izz*u22**2*sin(theta23(t))*cos(theta21(t))*cos(theta22(t)))/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2)) + (-2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t))) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))))*(g*l1*m1*sin(theta11(t))*cos(theta12(t)) + g*l1*m2*sin(theta11(t))*cos(theta12(t)) + 1.0*l1**2*m1*u11**2*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) + 1.0*l1**2*m1*u11**2*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 - 1.0*l1**2*m1*u11**2*sin(theta11(t))*cos(theta11(t)) + 1.0*l1**2*m1*u12**2*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) + 1.0*l1**2*m1*u12**2*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 + 1.0*l1**2*m2*u11**2*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) + 1.0*l1**2*m2*u11**2*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 - 1.0*l1**2*m2*u11**2*sin(theta11(t))*cos(theta11(t)) + 1.0*l1**2*m2*u12**2*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) + 1.0*l1**2*m2*u12**2*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 + 1.0*l1*l2*m2*u21**2*sin(theta11(t))*sin(theta12(t))*sin(theta22(t))*cos(theta21(t)) + 1.0*l1*l2*m2*u21**2*sin(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u21**2*sin(theta21(t))*cos(theta11(t)) + 2.0*l1*l2*m2*u21*u22*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*cos(theta22(t)) - 2.0*l1*l2*m2*u21*u22*sin(theta11(t))*sin(theta21(t))*sin(theta22(t))*cos(theta12(t)) + 1.0*l1*l2*m2*u22**2*sin(theta11(t))*sin(theta12(t))*sin(theta22(t))*cos(theta21(t)) + 1.0*l1*l2*m2*u22**2*sin(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2)) + (g*l1*m1*sin(theta12(t))*cos(theta11(t)) + g*l1*m2*sin(theta12(t))*cos(theta11(t)) - 2.0*l1**2*m1*u11*u12*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) - 2.0*l1**2*m1*u11*u12*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 - 2.0*l1**2*m2*u11*u12*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) - 2.0*l1**2*m2*u11*u12*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 + 1.0*l1*l2*m2*u21**2*sin(theta12(t))*cos(theta11(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u21**2*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))*cos(theta21(t)) - 2.0*l1*l2*m2*u21*u22*sin(theta12(t))*sin(theta21(t))*sin(theta22(t))*cos(theta11(t)) - 2.0*l1*l2*m2*u21*u22*sin(theta21(t))*cos(theta11(t))*cos(theta12(t))*cos(theta22(t)) + 1.0*l1*l2*m2*u22**2*sin(theta12(t))*cos(theta11(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u22**2*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))*cos(theta21(t)))*(-2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2 - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2))/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2))
udot21 (-2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2))*(-1.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*u21**2*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*u21**2*sin(theta21(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Ixx*u21**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 2.0*Ixx*u21*u22*sin(theta21(t))**2*sin(theta22(t))*cos(theta22(t))*cos(theta23(t))**2 + 2.0*Ixx*u21*u22*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) - 2.0*Ixx*u21*u23*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) - 2.0*Ixx*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Ixx*u21*u23*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Ixx*u21*u23*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2.0*Ixx*u22**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta22(t))*cos(theta23(t)) - 1.0*Ixx*u22**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) - 1.0*Ixx*u22**2*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*u22**2*sin(theta21(t))*sin(theta23(t))**2*cos(theta21(t))*cos(theta22(t)) + 1.0*Ixx*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t))**2 + 1.0*Ixx*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Ixx*u22**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Ixx*u22*u23*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 - 1.0*Ixx*u22*u23*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 4.0*Ixx*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*u22*u23*sin(theta23(t))**2*cos(theta21(t))**2 + 1.0*Ixx*u22*u23*cos(theta21(t))**2*cos(theta23(t))**2 - 2.0*Iyy*u21*u22*sin(theta22(t))*cos(theta22(t))*cos(theta23(t))**2 - 2.0*Iyy*u21*u23*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) + 2.0*Iyy*u22**2*sin(theta22(t))*sin(theta23(t))*cos(theta22(t))*cos(theta23(t)) + 1.0*Iyy*u22*u23*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Iyy*u22*u23*cos(theta22(t))**2*cos(theta23(t))**2 + 1.0*Izz*u21**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*u21**2*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*u21**2*sin(theta21(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Izz*u21**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2.0*Izz*u21*u22*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 2.0*Izz*u21*u22*sin(theta22(t))*cos(theta21(t))**2*cos(theta22(t))*cos(theta23(t))**2 + 2.0*Izz*u21*u23*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) + 2.0*Izz*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Izz*u21*u23*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Izz*u21*u23*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Izz*u22**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) + 1.0*Izz*u22**2*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*u22**2*sin(theta21(t))*sin(theta23(t))**2*cos(theta21(t))*cos(theta22(t)) - 1.0*Izz*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t))**2 - 1.0*Izz*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Izz*u22**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta22(t))*cos(theta23(t)) - 1.0*Izz*u22**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 1.0*Izz*u22*u23*sin(theta21(t))**2*sin(theta23(t))**2 + 1.0*Izz*u22*u23*sin(theta21(t))**2*cos(theta23(t))**2 + 4.0*Izz*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t)) + 1.0*Izz*u22*u23*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Izz*u22*u23*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Izz*u22*u23*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) - 1.0*Izz*u22*u23*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u23**2*sin(theta21(t))*cos(theta23(t)) + 1.0*Izz*u23**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)) + g*l2*m2*sin(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u11**2*sin(theta11(t))*cos(theta21(t)) + 1.0*l1*l2*m2*u11**2*sin(theta12(t))*sin(theta21(t))*sin(theta22(t))*cos(theta11(t)) + 1.0*l1*l2*m2*u11**2*sin(theta21(t))*cos(theta11(t))*cos(theta12(t))*cos(theta22(t)) - 2.0*l1*l2*m2*u11*u12*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*cos(theta22(t)) + 2.0*l1*l2*m2*u11*u12*sin(theta11(t))*sin(theta21(t))*sin(theta22(t))*cos(theta12(t)) + 1.0*l1*l2*m2*u12**2*sin(theta12(t))*sin(theta21(t))*sin(theta22(t))*cos(theta11(t)) + 1.0*l1*l2*m2*u12**2*sin(theta21(t))*cos(theta11(t))*cos(theta12(t))*cos(theta22(t)) + 1.0*l2**2*m2*u21**2*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t)) + 1.0*l2**2*m2*u21**2*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))**2 - 1.0*l2**2*m2*u21**2*sin(theta21(t))*cos(theta21(t)) + 1.0*l2**2*m2*u22**2*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t)) + 1.0*l2**2*m2*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))**2)/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2)) + (2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2))*(1.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))**2 - 1.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))*cos(theta22(t))*cos(theta23(t))**2 - 1.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))*cos(theta23(t))**2 - 2.0*Ixx*u21**2*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*u21**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) - 2.0*Ixx*u21**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*u21**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t))**2 + 1.0*Ixx*u21**2*sin(theta22(t))*cos(theta21(t))**2*cos(theta23(t))**2 + 2.0*Ixx*u21*u22*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) + 2.0*Ixx*u21*u22*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Ixx*u21*u22*sin(theta21(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Ixx*u21*u22*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Ixx*u21*u23*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 - 1.0*Ixx*u21*u23*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 4.0*Ixx*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*u21*u23*sin(theta23(t))**2*cos(theta21(t))**2 + 1.0*Ixx*u21*u23*cos(theta21(t))**2*cos(theta23(t))**2 + 1.0*Ixx*u22**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta22(t)) - 1.0*Ixx*u22**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 2.0*Ixx*u22*u23*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 2.0*Ixx*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Ixx*u22*u23*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Ixx*u22*u23*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*u21**2*sin(theta22(t))*cos(theta22(t))*cos(theta23(t))**2 + 1.0*Iyy*u21*u23*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Iyy*u21*u23*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Iyy*u22**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta22(t)) + 2.0*Iyy*u22*u23*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*u21**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))**2 + 1.0*Izz*u21**2*sin(theta21(t))**2*sin(theta22(t))*cos(theta23(t))**2 + 2.0*Izz*u21**2*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u21**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 2.0*Izz*u21**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u21**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Izz*u21**2*sin(theta22(t))*cos(theta21(t))**2*cos(theta22(t))*cos(theta23(t))**2 - 1.0*Izz*u21**2*sin(theta22(t))*cos(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*u21*u22*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) - 2.0*Izz*u21*u22*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Izz*u21*u22*sin(theta21(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Izz*u21*u22*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 1.0*Izz*u21*u23*sin(theta21(t))**2*sin(theta23(t))**2 + 1.0*Izz*u21*u23*sin(theta21(t))**2*cos(theta23(t))**2 + 4.0*Izz*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t)) + 1.0*Izz*u21*u23*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Izz*u21*u23*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 + 1.0*Izz*u21*u23*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 1.0*Izz*u21*u23*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u22**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 1.0*Izz*u22**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t))**2*cos(theta22(t)) - 2.0*Izz*u22*u23*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 2.0*Izz*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Izz*u22*u23*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Izz*u22*u23*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 1.0*Izz*u23**2*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*u23**2*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)) + g*l2*m2*sin(theta22(t))*cos(theta21(t)) - 1.0*l1*l2*m2*u11**2*sin(theta12(t))*cos(theta11(t))*cos(theta21(t))*cos(theta22(t)) + 1.0*l1*l2*m2*u11**2*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))*cos(theta21(t)) - 2.0*l1*l2*m2*u11*u12*sin(theta11(t))*sin(theta12(t))*sin(theta22(t))*cos(theta21(t)) - 2.0*l1*l2*m2*u11*u12*sin(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u12**2*sin(theta12(t))*cos(theta11(t))*cos(theta21(t))*cos(theta22(t)) + 1.0*l1*l2*m2*u12**2*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))*cos(theta21(t)) - 2.0*l2**2*m2*u21*u22*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t)) - 2.0*l2**2*m2*u21*u22*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))**2)/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2)) + (2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))) - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2))*(g*l1*m1*sin(theta12(t))*cos(theta11(t)) + g*l1*m2*sin(theta12(t))*cos(theta11(t)) - 2.0*l1**2*m1*u11*u12*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) - 2.0*l1**2*m1*u11*u12*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 - 2.0*l1**2*m2*u11*u12*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) - 2.0*l1**2*m2*u11*u12*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 + 1.0*l1*l2*m2*u21**2*sin(theta12(t))*cos(theta11(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u21**2*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))*cos(theta21(t)) - 2.0*l1*l2*m2*u21*u22*sin(theta12(t))*sin(theta21(t))*sin(theta22(t))*cos(theta11(t)) - 2.0*l1*l2*m2*u21*u22*sin(theta21(t))*cos(theta11(t))*cos(theta12(t))*cos(theta22(t)) + 1.0*l1*l2*m2*u22**2*sin(theta12(t))*cos(theta11(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u22**2*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))*cos(theta21(t)))/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2)) + ((-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))))*(-2.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) - 2.0*Ixx*u21**2*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Ixx*u21**2*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Ixx*u21**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 2.0*Ixx*u21*u22*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 - 2.0*Ixx*u21*u22*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 8.0*Ixx*u21*u22*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 2.0*Ixx*u21*u22*sin(theta23(t))**2*cos(theta21(t))**2 + 2.0*Ixx*u21*u22*cos(theta21(t))**2*cos(theta23(t))**2 + 2.0*Ixx*u22**2*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 2.0*Ixx*u22**2*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Ixx*u22**2*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Ixx*u22**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2.0*Iyy*u21**2*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) + 2.0*Iyy*u21*u22*sin(theta23(t))**2*cos(theta22(t))**2 - 2.0*Iyy*u21*u22*cos(theta22(t))**2*cos(theta23(t))**2 + 2.0*Iyy*u22**2*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) + 2.0*Izz*u21**2*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) + 2.0*Izz*u21**2*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Izz*u21**2*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 2*Izz*u21**2*sin(theta21(t))*sin(theta22(t))*cos(theta23(t)) - 2.0*Izz*u21**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2*Izz*u21**2*sin(theta23(t))*cos(theta21(t)) - 2.0*Izz*u21*u22*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*u21*u22*sin(theta21(t))**2*cos(theta23(t))**2 + 8.0*Izz*u21*u22*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) + 2*Izz*u21*u22*sin(theta21(t))*sin(theta22(t))*sin(theta23(t)) + 2.0*Izz*u21*u22*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 2.0*Izz*u21*u22*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 + 2*Izz*u21*u22*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) - 2*Izz*u21*u22*cos(theta21(t))*cos(theta23(t)) - 2.0*Izz*u22**2*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 2.0*Izz*u22**2*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Izz*u22**2*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Izz*u22**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2*Izz*u22**2*sin(theta23(t))*cos(theta21(t))*cos(theta22(t)))/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2)) + (-2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t))) + 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2))*(g*l1*m1*sin(theta11(t))*cos(theta12(t)) + g*l1*m2*sin(theta11(t))*cos(theta12(t)) + 1.0*l1**2*m1*u11**2*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) + 1.0*l1**2*m1*u11**2*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 - 1.0*l1**2*m1*u11**2*sin(theta11(t))*cos(theta11(t)) + 1.0*l1**2*m1*u12**2*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) + 1.0*l1**2*m1*u12**2*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 + 1.0*l1**2*m2*u11**2*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) + 1.0*l1**2*m2*u11**2*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 - 1.0*l1**2*m2*u11**2*sin(theta11(t))*cos(theta11(t)) + 1.0*l1**2*m2*u12**2*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) + 1.0*l1**2*m2*u12**2*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 + 1.0*l1*l2*m2*u21**2*sin(theta11(t))*sin(theta12(t))*sin(theta22(t))*cos(theta21(t)) + 1.0*l1*l2*m2*u21**2*sin(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u21**2*sin(theta21(t))*cos(theta11(t)) + 2.0*l1*l2*m2*u21*u22*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*cos(theta22(t)) - 2.0*l1*l2*m2*u21*u22*sin(theta11(t))*sin(theta21(t))*sin(theta22(t))*cos(theta12(t)) + 1.0*l1*l2*m2*u22**2*sin(theta11(t))*sin(theta12(t))*sin(theta22(t))*cos(theta21(t)) + 1.0*l1*l2*m2*u22**2*sin(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2))
udot22 (-2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2))*(1.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))**2 - 1.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))*cos(theta22(t))*cos(theta23(t))**2 - 1.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))*cos(theta23(t))**2 - 2.0*Ixx*u21**2*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*u21**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) - 2.0*Ixx*u21**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*u21**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t))**2 + 1.0*Ixx*u21**2*sin(theta22(t))*cos(theta21(t))**2*cos(theta23(t))**2 + 2.0*Ixx*u21*u22*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) + 2.0*Ixx*u21*u22*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Ixx*u21*u22*sin(theta21(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Ixx*u21*u22*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Ixx*u21*u23*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 - 1.0*Ixx*u21*u23*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 4.0*Ixx*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*u21*u23*sin(theta23(t))**2*cos(theta21(t))**2 + 1.0*Ixx*u21*u23*cos(theta21(t))**2*cos(theta23(t))**2 + 1.0*Ixx*u22**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta22(t)) - 1.0*Ixx*u22**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 2.0*Ixx*u22*u23*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 2.0*Ixx*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Ixx*u22*u23*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Ixx*u22*u23*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*u21**2*sin(theta22(t))*cos(theta22(t))*cos(theta23(t))**2 + 1.0*Iyy*u21*u23*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Iyy*u21*u23*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Iyy*u22**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta22(t)) + 2.0*Iyy*u22*u23*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*u21**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))**2 + 1.0*Izz*u21**2*sin(theta21(t))**2*sin(theta22(t))*cos(theta23(t))**2 + 2.0*Izz*u21**2*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u21**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 2.0*Izz*u21**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u21**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Izz*u21**2*sin(theta22(t))*cos(theta21(t))**2*cos(theta22(t))*cos(theta23(t))**2 - 1.0*Izz*u21**2*sin(theta22(t))*cos(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*u21*u22*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) - 2.0*Izz*u21*u22*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Izz*u21*u22*sin(theta21(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Izz*u21*u22*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 1.0*Izz*u21*u23*sin(theta21(t))**2*sin(theta23(t))**2 + 1.0*Izz*u21*u23*sin(theta21(t))**2*cos(theta23(t))**2 + 4.0*Izz*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t)) + 1.0*Izz*u21*u23*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Izz*u21*u23*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 + 1.0*Izz*u21*u23*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 1.0*Izz*u21*u23*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u22**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 1.0*Izz*u22**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t))**2*cos(theta22(t)) - 2.0*Izz*u22*u23*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 2.0*Izz*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Izz*u22*u23*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Izz*u22*u23*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 1.0*Izz*u23**2*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*u23**2*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)) + g*l2*m2*sin(theta22(t))*cos(theta21(t)) - 1.0*l1*l2*m2*u11**2*sin(theta12(t))*cos(theta11(t))*cos(theta21(t))*cos(theta22(t)) + 1.0*l1*l2*m2*u11**2*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))*cos(theta21(t)) - 2.0*l1*l2*m2*u11*u12*sin(theta11(t))*sin(theta12(t))*sin(theta22(t))*cos(theta21(t)) - 2.0*l1*l2*m2*u11*u12*sin(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u12**2*sin(theta12(t))*cos(theta11(t))*cos(theta21(t))*cos(theta22(t)) + 1.0*l1*l2*m2*u12**2*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))*cos(theta21(t)) - 2.0*l2**2*m2*u21*u22*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t)) - 2.0*l2**2*m2*u21*u22*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))**2)/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2)) + (2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2))*(-1.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*u21**2*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*u21**2*sin(theta21(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Ixx*u21**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 2.0*Ixx*u21*u22*sin(theta21(t))**2*sin(theta22(t))*cos(theta22(t))*cos(theta23(t))**2 + 2.0*Ixx*u21*u22*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) - 2.0*Ixx*u21*u23*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) - 2.0*Ixx*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Ixx*u21*u23*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Ixx*u21*u23*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2.0*Ixx*u22**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta22(t))*cos(theta23(t)) - 1.0*Ixx*u22**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) - 1.0*Ixx*u22**2*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*u22**2*sin(theta21(t))*sin(theta23(t))**2*cos(theta21(t))*cos(theta22(t)) + 1.0*Ixx*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t))**2 + 1.0*Ixx*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Ixx*u22**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Ixx*u22*u23*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 - 1.0*Ixx*u22*u23*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 4.0*Ixx*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*u22*u23*sin(theta23(t))**2*cos(theta21(t))**2 + 1.0*Ixx*u22*u23*cos(theta21(t))**2*cos(theta23(t))**2 - 2.0*Iyy*u21*u22*sin(theta22(t))*cos(theta22(t))*cos(theta23(t))**2 - 2.0*Iyy*u21*u23*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) + 2.0*Iyy*u22**2*sin(theta22(t))*sin(theta23(t))*cos(theta22(t))*cos(theta23(t)) + 1.0*Iyy*u22*u23*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Iyy*u22*u23*cos(theta22(t))**2*cos(theta23(t))**2 + 1.0*Izz*u21**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*u21**2*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*u21**2*sin(theta21(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Izz*u21**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2.0*Izz*u21*u22*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 2.0*Izz*u21*u22*sin(theta22(t))*cos(theta21(t))**2*cos(theta22(t))*cos(theta23(t))**2 + 2.0*Izz*u21*u23*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) + 2.0*Izz*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Izz*u21*u23*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Izz*u21*u23*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Izz*u22**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) + 1.0*Izz*u22**2*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*u22**2*sin(theta21(t))*sin(theta23(t))**2*cos(theta21(t))*cos(theta22(t)) - 1.0*Izz*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t))**2 - 1.0*Izz*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Izz*u22**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta22(t))*cos(theta23(t)) - 1.0*Izz*u22**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 1.0*Izz*u22*u23*sin(theta21(t))**2*sin(theta23(t))**2 + 1.0*Izz*u22*u23*sin(theta21(t))**2*cos(theta23(t))**2 + 4.0*Izz*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t)) + 1.0*Izz*u22*u23*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Izz*u22*u23*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Izz*u22*u23*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) - 1.0*Izz*u22*u23*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u23**2*sin(theta21(t))*cos(theta23(t)) + 1.0*Izz*u23**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)) + g*l2*m2*sin(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u11**2*sin(theta11(t))*cos(theta21(t)) + 1.0*l1*l2*m2*u11**2*sin(theta12(t))*sin(theta21(t))*sin(theta22(t))*cos(theta11(t)) + 1.0*l1*l2*m2*u11**2*sin(theta21(t))*cos(theta11(t))*cos(theta12(t))*cos(theta22(t)) - 2.0*l1*l2*m2*u11*u12*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*cos(theta22(t)) + 2.0*l1*l2*m2*u11*u12*sin(theta11(t))*sin(theta21(t))*sin(theta22(t))*cos(theta12(t)) + 1.0*l1*l2*m2*u12**2*sin(theta12(t))*sin(theta21(t))*sin(theta22(t))*cos(theta11(t)) + 1.0*l1*l2*m2*u12**2*sin(theta21(t))*cos(theta11(t))*cos(theta12(t))*cos(theta22(t)) + 1.0*l2**2*m2*u21**2*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t)) + 1.0*l2**2*m2*u21**2*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))**2 - 1.0*l2**2*m2*u21**2*sin(theta21(t))*cos(theta21(t)) + 1.0*l2**2*m2*u22**2*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t)) + 1.0*l2**2*m2*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))**2)/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2)) + (2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2))*(g*l1*m1*sin(theta11(t))*cos(theta12(t)) + g*l1*m2*sin(theta11(t))*cos(theta12(t)) + 1.0*l1**2*m1*u11**2*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) + 1.0*l1**2*m1*u11**2*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 - 1.0*l1**2*m1*u11**2*sin(theta11(t))*cos(theta11(t)) + 1.0*l1**2*m1*u12**2*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) + 1.0*l1**2*m1*u12**2*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 + 1.0*l1**2*m2*u11**2*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) + 1.0*l1**2*m2*u11**2*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 - 1.0*l1**2*m2*u11**2*sin(theta11(t))*cos(theta11(t)) + 1.0*l1**2*m2*u12**2*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) + 1.0*l1**2*m2*u12**2*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 + 1.0*l1*l2*m2*u21**2*sin(theta11(t))*sin(theta12(t))*sin(theta22(t))*cos(theta21(t)) + 1.0*l1*l2*m2*u21**2*sin(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u21**2*sin(theta21(t))*cos(theta11(t)) + 2.0*l1*l2*m2*u21*u22*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*cos(theta22(t)) - 2.0*l1*l2*m2*u21*u22*sin(theta11(t))*sin(theta21(t))*sin(theta22(t))*cos(theta12(t)) + 1.0*l1*l2*m2*u22**2*sin(theta11(t))*sin(theta12(t))*sin(theta22(t))*cos(theta21(t)) + 1.0*l1*l2*m2*u22**2*sin(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2)) + (-2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2))*(g*l1*m1*sin(theta12(t))*cos(theta11(t)) + g*l1*m2*sin(theta12(t))*cos(theta11(t)) - 2.0*l1**2*m1*u11*u12*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) - 2.0*l1**2*m1*u11*u12*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 - 2.0*l1**2*m2*u11*u12*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) - 2.0*l1**2*m2*u11*u12*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 + 1.0*l1*l2*m2*u21**2*sin(theta12(t))*cos(theta11(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u21**2*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))*cos(theta21(t)) - 2.0*l1*l2*m2*u21*u22*sin(theta12(t))*sin(theta21(t))*sin(theta22(t))*cos(theta11(t)) - 2.0*l1*l2*m2*u21*u22*sin(theta21(t))*cos(theta11(t))*cos(theta12(t))*cos(theta22(t)) + 1.0*l1*l2*m2*u22**2*sin(theta12(t))*cos(theta11(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u22**2*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))*cos(theta21(t)))/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2)) + (-(-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2))*(-2.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) - 2.0*Ixx*u21**2*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Ixx*u21**2*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Ixx*u21**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 2.0*Ixx*u21*u22*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 - 2.0*Ixx*u21*u22*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 8.0*Ixx*u21*u22*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 2.0*Ixx*u21*u22*sin(theta23(t))**2*cos(theta21(t))**2 + 2.0*Ixx*u21*u22*cos(theta21(t))**2*cos(theta23(t))**2 + 2.0*Ixx*u22**2*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 2.0*Ixx*u22**2*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Ixx*u22**2*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Ixx*u22**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2.0*Iyy*u21**2*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) + 2.0*Iyy*u21*u22*sin(theta23(t))**2*cos(theta22(t))**2 - 2.0*Iyy*u21*u22*cos(theta22(t))**2*cos(theta23(t))**2 + 2.0*Iyy*u22**2*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) + 2.0*Izz*u21**2*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) + 2.0*Izz*u21**2*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Izz*u21**2*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 2*Izz*u21**2*sin(theta21(t))*sin(theta22(t))*cos(theta23(t)) - 2.0*Izz*u21**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2*Izz*u21**2*sin(theta23(t))*cos(theta21(t)) - 2.0*Izz*u21*u22*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*u21*u22*sin(theta21(t))**2*cos(theta23(t))**2 + 8.0*Izz*u21*u22*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) + 2*Izz*u21*u22*sin(theta21(t))*sin(theta22(t))*sin(theta23(t)) + 2.0*Izz*u21*u22*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 2.0*Izz*u21*u22*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 + 2*Izz*u21*u22*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) - 2*Izz*u21*u22*cos(theta21(t))*cos(theta23(t)) - 2.0*Izz*u22**2*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 2.0*Izz*u22**2*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Izz*u22**2*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Izz*u22**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2*Izz*u22**2*sin(theta23(t))*cos(theta21(t))*cos(theta22(t)))/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2))
udot23 ((2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))))*(-1.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*u21**2*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*u21**2*sin(theta21(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Ixx*u21**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 2.0*Ixx*u21*u22*sin(theta21(t))**2*sin(theta22(t))*cos(theta22(t))*cos(theta23(t))**2 + 2.0*Ixx*u21*u22*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) - 2.0*Ixx*u21*u23*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) - 2.0*Ixx*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Ixx*u21*u23*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Ixx*u21*u23*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2.0*Ixx*u22**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta22(t))*cos(theta23(t)) - 1.0*Ixx*u22**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) - 1.0*Ixx*u22**2*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*u22**2*sin(theta21(t))*sin(theta23(t))**2*cos(theta21(t))*cos(theta22(t)) + 1.0*Ixx*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t))**2 + 1.0*Ixx*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Ixx*u22**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Ixx*u22*u23*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 - 1.0*Ixx*u22*u23*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 4.0*Ixx*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*u22*u23*sin(theta23(t))**2*cos(theta21(t))**2 + 1.0*Ixx*u22*u23*cos(theta21(t))**2*cos(theta23(t))**2 - 2.0*Iyy*u21*u22*sin(theta22(t))*cos(theta22(t))*cos(theta23(t))**2 - 2.0*Iyy*u21*u23*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) + 2.0*Iyy*u22**2*sin(theta22(t))*sin(theta23(t))*cos(theta22(t))*cos(theta23(t)) + 1.0*Iyy*u22*u23*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Iyy*u22*u23*cos(theta22(t))**2*cos(theta23(t))**2 + 1.0*Izz*u21**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*u21**2*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*u21**2*sin(theta21(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Izz*u21**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2.0*Izz*u21*u22*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 2.0*Izz*u21*u22*sin(theta22(t))*cos(theta21(t))**2*cos(theta22(t))*cos(theta23(t))**2 + 2.0*Izz*u21*u23*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) + 2.0*Izz*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Izz*u21*u23*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Izz*u21*u23*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Izz*u22**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) + 1.0*Izz*u22**2*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*u22**2*sin(theta21(t))*sin(theta23(t))**2*cos(theta21(t))*cos(theta22(t)) - 1.0*Izz*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t))**2 - 1.0*Izz*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Izz*u22**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta22(t))*cos(theta23(t)) - 1.0*Izz*u22**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 1.0*Izz*u22*u23*sin(theta21(t))**2*sin(theta23(t))**2 + 1.0*Izz*u22*u23*sin(theta21(t))**2*cos(theta23(t))**2 + 4.0*Izz*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t)) + 1.0*Izz*u22*u23*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Izz*u22*u23*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Izz*u22*u23*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) - 1.0*Izz*u22*u23*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u23**2*sin(theta21(t))*cos(theta23(t)) + 1.0*Izz*u23**2*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)) + g*l2*m2*sin(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u11**2*sin(theta11(t))*cos(theta21(t)) + 1.0*l1*l2*m2*u11**2*sin(theta12(t))*sin(theta21(t))*sin(theta22(t))*cos(theta11(t)) + 1.0*l1*l2*m2*u11**2*sin(theta21(t))*cos(theta11(t))*cos(theta12(t))*cos(theta22(t)) - 2.0*l1*l2*m2*u11*u12*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*cos(theta22(t)) + 2.0*l1*l2*m2*u11*u12*sin(theta11(t))*sin(theta21(t))*sin(theta22(t))*cos(theta12(t)) + 1.0*l1*l2*m2*u12**2*sin(theta12(t))*sin(theta21(t))*sin(theta22(t))*cos(theta11(t)) + 1.0*l1*l2*m2*u12**2*sin(theta21(t))*cos(theta11(t))*cos(theta12(t))*cos(theta22(t)) + 1.0*l2**2*m2*u21**2*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t)) + 1.0*l2**2*m2*u21**2*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))**2 - 1.0*l2**2*m2*u21**2*sin(theta21(t))*cos(theta21(t)) + 1.0*l2**2*m2*u22**2*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t)) + 1.0*l2**2*m2*u22**2*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))**2)/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2)) + (-(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2))*(1.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))**2 - 1.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))*cos(theta22(t))*cos(theta23(t))**2 - 1.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))*cos(theta23(t))**2 - 2.0*Ixx*u21**2*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*u21**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) - 2.0*Ixx*u21**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*u21**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t))**2 + 1.0*Ixx*u21**2*sin(theta22(t))*cos(theta21(t))**2*cos(theta23(t))**2 + 2.0*Ixx*u21*u22*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) + 2.0*Ixx*u21*u22*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Ixx*u21*u22*sin(theta21(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Ixx*u21*u22*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Ixx*u21*u23*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 - 1.0*Ixx*u21*u23*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 4.0*Ixx*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*u21*u23*sin(theta23(t))**2*cos(theta21(t))**2 + 1.0*Ixx*u21*u23*cos(theta21(t))**2*cos(theta23(t))**2 + 1.0*Ixx*u22**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta22(t)) - 1.0*Ixx*u22**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 2.0*Ixx*u22*u23*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 2.0*Ixx*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Ixx*u22*u23*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Ixx*u22*u23*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*u21**2*sin(theta22(t))*cos(theta22(t))*cos(theta23(t))**2 + 1.0*Iyy*u21*u23*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Iyy*u21*u23*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Iyy*u22**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta22(t)) + 2.0*Iyy*u22*u23*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*u21**2*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))**2 + 1.0*Izz*u21**2*sin(theta21(t))**2*sin(theta22(t))*cos(theta23(t))**2 + 2.0*Izz*u21**2*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u21**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 2.0*Izz*u21**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u21**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Izz*u21**2*sin(theta22(t))*cos(theta21(t))**2*cos(theta22(t))*cos(theta23(t))**2 - 1.0*Izz*u21**2*sin(theta22(t))*cos(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*u21*u22*sin(theta21(t))**2*sin(theta22(t))*sin(theta23(t))*cos(theta23(t)) - 2.0*Izz*u21*u22*sin(theta21(t))*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Izz*u21*u22*sin(theta21(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Izz*u21*u22*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 1.0*Izz*u21*u23*sin(theta21(t))**2*sin(theta23(t))**2 + 1.0*Izz*u21*u23*sin(theta21(t))**2*cos(theta23(t))**2 + 4.0*Izz*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*u21*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t)) + 1.0*Izz*u21*u23*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Izz*u21*u23*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 + 1.0*Izz*u21*u23*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 1.0*Izz*u21*u23*cos(theta21(t))*cos(theta23(t)) + 1.0*Izz*u22**2*sin(theta21(t))*sin(theta23(t))*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) + 1.0*Izz*u22**2*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t))**2*cos(theta22(t)) - 2.0*Izz*u22*u23*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 2.0*Izz*u22*u23*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Izz*u22*u23*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Izz*u22*u23*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 1.0*Izz*u23**2*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*u23**2*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)) + g*l2*m2*sin(theta22(t))*cos(theta21(t)) - 1.0*l1*l2*m2*u11**2*sin(theta12(t))*cos(theta11(t))*cos(theta21(t))*cos(theta22(t)) + 1.0*l1*l2*m2*u11**2*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))*cos(theta21(t)) - 2.0*l1*l2*m2*u11*u12*sin(theta11(t))*sin(theta12(t))*sin(theta22(t))*cos(theta21(t)) - 2.0*l1*l2*m2*u11*u12*sin(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u12**2*sin(theta12(t))*cos(theta11(t))*cos(theta21(t))*cos(theta22(t)) + 1.0*l1*l2*m2*u12**2*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))*cos(theta21(t)) - 2.0*l2**2*m2*u21*u22*sin(theta21(t))*sin(theta22(t))**2*cos(theta21(t)) - 2.0*l2**2*m2*u21*u22*sin(theta21(t))*cos(theta21(t))*cos(theta22(t))**2)/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2)) + (-(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) - (2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 + (2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2))*(g*l1*m1*sin(theta12(t))*cos(theta11(t)) + g*l1*m2*sin(theta12(t))*cos(theta11(t)) - 2.0*l1**2*m1*u11*u12*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) - 2.0*l1**2*m1*u11*u12*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 - 2.0*l1**2*m2*u11*u12*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) - 2.0*l1**2*m2*u11*u12*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 + 1.0*l1*l2*m2*u21**2*sin(theta12(t))*cos(theta11(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u21**2*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))*cos(theta21(t)) - 2.0*l1*l2*m2*u21*u22*sin(theta12(t))*sin(theta21(t))*sin(theta22(t))*cos(theta11(t)) - 2.0*l1*l2*m2*u21*u22*sin(theta21(t))*cos(theta11(t))*cos(theta12(t))*cos(theta22(t)) + 1.0*l1*l2*m2*u22**2*sin(theta12(t))*cos(theta11(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u22**2*sin(theta22(t))*cos(theta11(t))*cos(theta12(t))*cos(theta21(t)))/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2)) + ((2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 + (2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) + (2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) - (2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))))*(g*l1*m1*sin(theta11(t))*cos(theta12(t)) + g*l1*m2*sin(theta11(t))*cos(theta12(t)) + 1.0*l1**2*m1*u11**2*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) + 1.0*l1**2*m1*u11**2*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 - 1.0*l1**2*m1*u11**2*sin(theta11(t))*cos(theta11(t)) + 1.0*l1**2*m1*u12**2*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) + 1.0*l1**2*m1*u12**2*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 + 1.0*l1**2*m2*u11**2*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) + 1.0*l1**2*m2*u11**2*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 - 1.0*l1**2*m2*u11**2*sin(theta11(t))*cos(theta11(t)) + 1.0*l1**2*m2*u12**2*sin(theta11(t))*sin(theta12(t))**2*cos(theta11(t)) + 1.0*l1**2*m2*u12**2*sin(theta11(t))*cos(theta11(t))*cos(theta12(t))**2 + 1.0*l1*l2*m2*u21**2*sin(theta11(t))*sin(theta12(t))*sin(theta22(t))*cos(theta21(t)) + 1.0*l1*l2*m2*u21**2*sin(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*u21**2*sin(theta21(t))*cos(theta11(t)) + 2.0*l1*l2*m2*u21*u22*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*cos(theta22(t)) - 2.0*l1*l2*m2*u21*u22*sin(theta11(t))*sin(theta21(t))*sin(theta22(t))*cos(theta12(t)) + 1.0*l1*l2*m2*u22**2*sin(theta11(t))*sin(theta12(t))*sin(theta22(t))*cos(theta21(t)) + 1.0*l1*l2*m2*u22**2*sin(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2)) + ((1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - (1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 2*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - (-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - (-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + (-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - (-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2)*(-2.0*Ixx*u21**2*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) - 2.0*Ixx*u21**2*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Ixx*u21**2*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Ixx*u21**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 2.0*Ixx*u21*u22*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 - 2.0*Ixx*u21*u22*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 8.0*Ixx*u21*u22*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 2.0*Ixx*u21*u22*sin(theta23(t))**2*cos(theta21(t))**2 + 2.0*Ixx*u21*u22*cos(theta21(t))**2*cos(theta23(t))**2 + 2.0*Ixx*u22**2*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 2.0*Ixx*u22**2*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Ixx*u22**2*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 2.0*Ixx*u22**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2.0*Iyy*u21**2*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) + 2.0*Iyy*u21*u22*sin(theta23(t))**2*cos(theta22(t))**2 - 2.0*Iyy*u21*u22*cos(theta22(t))**2*cos(theta23(t))**2 + 2.0*Iyy*u22**2*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) + 2.0*Izz*u21**2*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) + 2.0*Izz*u21**2*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 2.0*Izz*u21**2*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 2*Izz*u21**2*sin(theta21(t))*sin(theta22(t))*cos(theta23(t)) - 2.0*Izz*u21**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2*Izz*u21**2*sin(theta23(t))*cos(theta21(t)) - 2.0*Izz*u21*u22*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*u21*u22*sin(theta21(t))**2*cos(theta23(t))**2 + 8.0*Izz*u21*u22*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) + 2*Izz*u21*u22*sin(theta21(t))*sin(theta22(t))*sin(theta23(t)) + 2.0*Izz*u21*u22*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 2.0*Izz*u21*u22*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 + 2*Izz*u21*u22*cos(theta21(t))*cos(theta22(t))*cos(theta23(t)) - 2*Izz*u21*u22*cos(theta21(t))*cos(theta23(t)) - 2.0*Izz*u22**2*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 2.0*Izz*u22**2*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 2.0*Izz*u22**2*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 2.0*Izz*u22**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) - 2*Izz*u22**2*sin(theta23(t))*cos(theta21(t))*cos(theta22(t)))/(2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2 - 2*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t))) + 4*Izz*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 4*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2 - 2*Izz*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) + 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2) - 2*Izz*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)))**2 + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))**2 + 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*Iyy*sin(theta23(t))**2*cos(theta22(t))**2 - 1.0*Izz*sin(theta21(t))**2*cos(theta23(t))**2 - 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*sin(theta22(t))**2*cos(theta21(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2*cos(theta22(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*sin(theta23(t)) + 1.0*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*cos(theta21(t))*cos(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta22(t))*cos(theta12(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta22(t))*cos(theta11(t))*cos(theta21(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta12(t))*cos(theta21(t))*cos(theta22(t)))*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (2*Izz*sin(theta21(t))*sin(theta23(t)) - 2*Izz*sin(theta22(t))*cos(theta21(t))*cos(theta23(t)))*(-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*sin(theta23(t))*cos(theta23(t)) + 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) - 1.0*Ixx*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 - 1.0*Ixx*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t)) + 1.0*Iyy*sin(theta23(t))*cos(theta22(t))**2*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))*cos(theta23(t)) - 1.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))**2*cos(theta21(t)) + 1.0*Izz*sin(theta21(t))*sin(theta22(t))*cos(theta21(t))*cos(theta23(t))**2 + 1.0*Izz*sin(theta22(t))**2*sin(theta23(t))*cos(theta21(t))**2*cos(theta23(t))) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta12(t))*sin(theta21(t))*cos(theta11(t))*cos(theta22(t)) + 1.0*l1*l2*m2*sin(theta21(t))*sin(theta22(t))*cos(theta11(t))*cos(theta12(t)))**2*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2) + (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1*l2*m2*sin(theta11(t))*sin(theta12(t))*sin(theta21(t))*sin(theta22(t)) - 1.0*l1*l2*m2*sin(theta11(t))*sin(theta21(t))*cos(theta12(t))*cos(theta22(t)) - 1.0*l1*l2*m2*cos(theta11(t))*cos(theta21(t)))**2*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2) - (-1.0*Izz*sin(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(2*Izz*sin(theta21(t))*cos(theta23(t)) + 2*Izz*sin(theta22(t))*sin(theta23(t))*cos(theta21(t)))*(-1.0*l1**2*m1*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*sin(theta12(t))**2*cos(theta11(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2*cos(theta12(t))**2)*(-1.0*l1**2*m1*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m1*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m1*cos(theta11(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*sin(theta12(t))**2 - 1.0*l1**2*m2*sin(theta11(t))**2*cos(theta12(t))**2 - 1.0*l1**2*m2*cos(theta11(t))**2)*(-1.0*Ixx*sin(theta21(t))**2*sin(theta22(t))**2*cos(theta23(t))**2 - 2.0*Ixx*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Ixx*sin(theta23(t))**2*cos(theta21(t))**2 - 1.0*Iyy*cos(theta22(t))**2*cos(theta23(t))**2 - 1.0*Izz*sin(theta21(t))**2*sin(theta23(t))**2 + 2.0*Izz*sin(theta21(t))*sin(theta22(t))*sin(theta23(t))*cos(theta21(t))*cos(theta23(t)) - 1.0*Izz*sin(theta22(t))**2*cos(theta21(t))**2*cos(theta23(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*sin(theta22(t))**2 - 1.0*l2**2*m2*sin(theta21(t))**2*cos(theta22(t))**2 - 1.0*l2**2*m2*cos(theta21(t))**2))

""" Calculate Rotation Matrices for Dist. Mass Model """
from sympy import symbols, Matrix
from sympy import sin, cos
from sympy import lambdify
from sympy import init_printing, latex, pprint

init_printing()

""" Define Symbols """
theta21, theta22, theta23 = symbols("theta21 theta22 theta23")
dtheta21, dtheta22, dtheta23 = symbols("dtheta21 dtheta22 dtheta23")
symbolnames = {
    dtheta21: r"\dot{\theta}_{21}",
    dtheta22: r"\dot{\theta}_{22}",
    dtheta23: r"\dot{\theta}_{23}",
}
ix = Matrix([1, 0, 0])
iy = Matrix([0, 1, 0])
iz = Matrix([0, 0, 1])

""" Define Rotation Matrices for each theta, note negative theta21 """
Ry = Matrix([[cos(-theta21), 0, sin(-theta21)],
            [0, 1, 0],
            [-sin(-theta21), 0, cos(-theta21)]])
Rx = Matrix([[1, 0, 0],
            [0, cos(theta22), -sin(theta22)],
            [0, sin(theta22), cos(theta22)]])
Rz = Matrix([[cos(theta23), -sin(theta23), 0],
            [sin(theta23), cos(theta23), 0],
            [0, 0, 1]])

""" Combined forward and inverse Rotation Matrices """
R = Ry * Rx * Rz
Rinv = Rz * Rx * Ry

""" dtheta's as Columnvectors, note negation of dtheta21 """
X = Matrix([dtheta22, 0, 0])
Y = Matrix([0, -dtheta21, 0])
Z = Matrix([0, 0, dtheta23])

""" Output conversion of dthetas and CoG-XY-Vectors """
for expr in [R * X + R * Y + Z, R * ix, R * iy]:
    print(latex(expr, symbol_name=symbolnames))
    print(expr)

""" Sandbox used for validating rotations """
thetas = [5, 3, 2]
negthetas = [t * -1 for t in thetas]
forw = lambdify([theta21, theta22, theta23], R)
back = lambdify([theta21, theta22, theta23], Rinv)
res = forw(*thetas) * ix
# pprint(res)
# pprint(back(*negthetas) * res)

"""Simulate 3D Distributed Mass Double-Pendulum

Uses better ode solver than the original 2d code its based off.
Dumps results as .npz for visualization in Grasshopper.
"""
import sys
import numpy as np
from math import isclose, sin, cos
from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from matplotlib.patches import Circle
from mpl_toolkits.mplot3d import Axes3D

# Pendulum rod lengths (m), bob masses (kg).
l1, l2 = 1, 1
m1, m2 = 1, 1
Ixx, Iyy, Izz = 1, 1, 1
# The gravitational acceleration (m.s-2).
g = 9.81
# Maximum time, time point spacings and the time grid (all in s).
tmax, dt = 30, 0.01
t = np.arange(0, tmax + dt, dt)
# Initial conditions: theta1, dtheta1/dt, theta2, dtheta2/dt.
y0_small = np.array(
    [
        np.pi / 8,  #  theta11
        0,  # dtheta11
        0,  #  theta12
        0,  # dtheta12
        np.pi / 8,  #  theta21
        0,  # dtheta21
        0,  #  theta22
        0,  # dtheta22
        np.pi / 4,  #  theta23
        1,  # dtheta23
    ]
)

# Plot settings
r = 0.05  # bob circle radius
fps = 25
di = int(1 / fps / dt)

def deriv(t, y, l1, l2, m1, m2):
    """Return the first derivatives of y = theta_ij, dtheta_ij"""
    theta11, u11, theta12, u12, theta21, u21, theta22, u22, theta23, u23 = y

    dtheta11 = u11
    dtheta12 = u12
    dtheta21 = u21
    dtheta22 = u22
    dtheta23 = u23
    du11 = 2 * Izz  # see above for full eom
    du12 = 2 * Izz  # see above for full eom
    du21 = -2 * Izz  # see above for full eom
    du22 = -2 * Izz  # see above for full eom
    du23 = 2 * Izz  # see above for full eom

    return [
        dtheta11,
        du11,
        dtheta12,
        du12,
        dtheta21,
        du21,
        dtheta22,
        du22,
        dtheta23,
        du23,
    ]

def plot_motion(t, y, title="", save=False):
    y = np.array(y).T
    theta11 = y[:, 0]
    theta12 = y[:, 2]
    theta21 = y[:, 4]
    theta22 = y[:, 6]

    # Convert to Cartesian coordinates of the two bob positions.
    x1 = l1 * np.sin(theta11)
    y1 = l1 * np.cos(theta11) * np.sin(theta12)
    z1 = -l1 * np.cos(theta11) * np.cos(theta12)
    x2 = x1 + l2 * np.sin(theta21)
    y2 = y1 + l2 * np.cos(theta21) * np.sin(theta22)
    z2 = z1 - l2 * np.cos(theta21) * np.cos(theta22)

    length1 = (isclose(np.linalg.norm([x, y, z]), l1) for x, y, z in zip(x1, y1, z1))
    length2 = (
        isclose(np.linalg.norm([x, y, z]), l2)
        for x, y, z in zip(x2 - x1, y2 - y1, z2 - z1)
    )
    if not (all(length1) and all(length2)):
        print("ERROR: Lengths of links are not maintained!")
        sys.exit()

    # Create animated plot
    fig = plt.figure(figsize=(12, 4))
    pltsize = 1.2 * (l1 + l2)
    axs, lines, trails, circles = [], [], [], []
    for i in range(3):
        ax = fig.add_subplot(
            131 + i,
            autoscale_on=False,
            xlim=(-pltsize, pltsize),
            ylim=(-pltsize, pltsize),
        )

        # Trail
        trail, = ax.plot(
            [], [], c="r", solid_capstyle="butt", lw=2, alpha=0.4, linestyle=":"
        )
        # The pendulum rods.
        line, = ax.plot([], [], c="k", lw=2)
        # Circles representing the anchor point of rod 1, and bobs 1 and 2.
        c0 = Circle((0, 0), r / 2, fc="k", zorder=10)
        c1 = Circle((0, 0), r, fc="b", ec="b", zorder=10)
        c2 = Circle((0, 0), r, fc="r", ec="r", zorder=10)
        for col, single in zip(
            [axs, lines, trails, circles], [ax, line, trail, [c0, c1, c2]]
        ):
            col.append(single)
    # Time label
    time_template = "time = %.1fs"
    time_text = axs[0].text(0.05, 0.9, "", transform=ax.transAxes)
    plt.tight_layout()

    def animation_init():
        for ax, line, trail, circs in zip(axs, lines, trails, circles):
            line.set_data([], [])
            for c in circs:
                ax.add_patch(c)
            trail.set_data([], [])
            time_text.set_text("")
        return axs + lines + trails + [c for cs in circles for c in cs] + [time_text]

    def animation_func(i):
        front = {"x1": x1, "x2": x2, "y1": z1, "y2": z2}
        right = {"x1": y1, "x2": y2, "y1": z1, "y2": z2}
        top = {"x1": x1, "x2": x2, "y1": y1, "y2": y2}
        views = [front, right, top]

        for view, ax, line, trail, circs in zip(views, axs, lines, trails, circles):
            thisx = [0, view["x1"][i], view["x2"][i]]
            thisy = [0, view["y1"][i], view["y2"][i]]
            line.set_data(thisx, thisy)
            circs[1].center = (thisx[1], thisy[1])
            circs[2].center = (thisx[2], thisy[2])
            trail.set_data(view["x2"][1:i], view["y2"][1:i])
        time_text.set_text(time_template % (i * dt))
        return axs + lines + trails + [c for cs in circles for c in cs] + [time_text]

    # blit=True sadly leads to blank animations on saving
    ani = animation.FuncAnimation(
        fig,
        animation_func,
        range(0, len(t), di),
        interval=fps,
        blit=False,
        init_func=animation_init,
    )
    if save:
        ani.save(
            "../output/double_pendulum_3d_distmass_%s.mp4"
            % title.lowercase().replace(" ", "_"),
            animation.writers["ffmpeg"](fps=fps),
            dpi=300,
        )
    plt.show()

def plot_angles(time, y, events):
    fig, axs = plt.subplots(2, 1, sharex=True)
    colors = ["C0", "C1", "C2", "C3"]
    linetypes = ["-", "--", "-.", ":"]
    labels = {
        "3d": [
            r"$\theta_{11}$",
            r"$\theta_{12}$",
            r"$\theta_{21}$",
            r"$\theta_{22}$",
            r"$\theta_{23}$",
        ],
        "3ddot": [
            r"$\dot{\theta}_{11}$",
            r"$\dot{\theta}_{12}$",
            r"$\dot{\theta}_{21}$",
            r"$\dot{\theta}_{22}$",
            r"$\dot{\theta}_{23}$",
        ],
    }

    axs[0].set_title("Upper Link", loc="left")
    axs[1].set_title("Lower Link", loc="left")
    for i, thetaid in zip([0, 1], [0, 1]):
        axs[0].plot(
            time,
            y[thetaid * 2, :] * 180 / np.pi,
            linetypes[i],
            color=colors[i],
            label=labels["3d"][thetaid],
            linewidth="0.85",
        )
    for i, thetaid in zip([0, 1, 2], [2, 3, 4]):
        axs[1].plot(
            time,
            y[thetaid * 2, :] * 180 / np.pi,
            linetypes[i],
            color=colors[i],
            label=labels["3d"][thetaid],
            linewidth="0.85",
        )
    for ax in axs:
        ax.legend(loc=1, framealpha=1)
        ax.grid(axis="y")
    plt.show()

def save2file(y, filename):
    y = np.array(y).T
    theta11 = y[:, 0]
    theta12 = y[:, 2]
    theta21 = y[:, 4]
    theta22 = y[:, 6]
    theta23 = y[:, 8]

    # Convert to Cartesian coordinates of the two bob positions.
    x1 = l1 * np.sin(theta11)
    y1 = l1 * np.cos(theta11) * np.sin(theta12)
    z1 = -l1 * np.cos(theta11) * np.cos(theta12)
    x2 = x1 + l2 * np.sin(theta21)
    y2 = y1 + l2 * np.cos(theta21) * np.sin(theta22)
    z2 = z1 - l2 * np.cos(theta21) * np.cos(theta22)
    points = np.vstack([x1, y1, z1, x2, y2, z2]).T

    # X and Y Vector of distributed mass reference frame
    # (equations generated by cog-rotations.py)
    cogxx = -np.sin(theta21) * np.sin(theta22) * np.sin(theta23) + np.cos(
        theta21
    ) * np.cos(theta23)
    cogxy = np.sin(theta23) * np.cos(theta22)
    cogxz = np.sin(theta21) * np.cos(theta23) + np.sin(theta22) * np.sin(
        theta23
    ) * np.cos(theta21)
    cogyx = -np.sin(theta21) * np.sin(theta22) * np.cos(theta23) - np.sin(
        theta23
    ) * np.cos(theta21)
    cogyy = np.cos(theta22) * np.cos(theta23)
    cogyz = -np.sin(theta21) * np.sin(theta23) + np.sin(theta22) * np.cos(
        theta21
    ) * np.cos(theta23)
    cogvecs = np.vstack([cogxx, cogxy, cogxz, cogyx, cogyy, cogyz]).T

    np.savetxt(filename, np.hstack([points, cogvecs]))

# Small Exitation
save_animations = False
sol = solve_ivp(lambda t, y: deriv(t, y, l1, l2, m1, m2), (0, tmax), y0_small, t_eval=t)
save2file(sol.y, "data4gh")
# plot_angles(t, sol.y, sol.t_events)
# plot_motion(sol.t, sol.y, title="Small Exitation", save=save_animations)
# np.savez('../output/distmass-double-pendulum-small_exitation', t=sol.t, y0=y0_small, y=sol.y)
print("done")

"""Load simulated pendulum data in GH """

import rhinoscriptsyntax as rs

m1Points = []
m2Planes = []
if not reload:
    f = open(fname)
    for l in f:
        raw = l.strip().split(" ")
        vals = [float(v) * 1000 for v in raw]
        m1Points.append(rs.coerce3dpoint(vals[:3]))
        m2Planes.append(rs.PlaneFromFrame(vals[3:6], vals[6:9], vals[9:12]))
    f.close()

10.5 Roof Array Workspace

The code below has been exported from an interactive Jupyter notebook that was used to generate the plots of the roof array momentum envelope for different roof angles.

""" Plot momentum envelope of a 4CMG roof array """

import plotly.offline as py
import plotly.graph_objs as go
from math import cos, sin, pi, sqrt
import numpy as np
import colorlover as cl
from IPython.display import HTML
py.init_notebook_mode()

def unit_sphere(theta1, theta2):
    i = np.array([1, 0, 0])
    j = np.array([0, 1, 0])
    k = np.array([0, 0, 1])
    return np.array(sin(theta2)*i
                    - sin(theta1) * cos(theta2) * j
                    + cos(theta1) * cos(theta2) * k)
#roof_angle = 0.52  #30 degrees
roof_angle = 0.78  #45 degrees

roof4_gimbal_axes = [np.array([0, -cos(roof_angle), sin(roof_angle)]),
                     np.array([0, -cos(roof_angle), sin(roof_angle)]),
                     np.array([0,  cos(roof_angle), sin(roof_angle)]),
                     np.array([0,  cos(roof_angle), sin(roof_angle)])]

epsilon_4H = [1, 1, 1, 1]
epsilon_0H = [1, 1, -1, -1]

def singular_momentum_vector(gimbal_axes, epsilon, theta1, theta2):
    H = np.zeros(3)
    u = unit_sphere(theta1, theta2)
    for i, g in enumerate(gimbal_axes):
        H_i = epsilon[i] * (u - g * np.dot(g, u))
        H_i /= np.linalg.norm(np.cross(g, u))
        H = H + H_i
    return H

res = 150
angles = np.linspace(0, 2*pi, res)

envelope = np.empty([res**2, 3])
inner = np.empty([res**2, 3])

n = 0
for i in angles:
    for j in angles:
        envelope[n] = singular_momentum_vector(roof4_gimbal_axes, epsilon_4H, i, j)
        inner[n] = singular_momentum_vector(roof4_gimbal_axes, epsilon_0H, i, j)
        n = n+1

cl1 = cl.scales['9']['seq']['Greens'][3:]
cl2 = cl.scales['9']['seq']['Oranges'][3:]

cscale1 = [ [ float(i)/float(len(cl1)-1), cl1[i] ]
                for i in range(len(cl1)) ]
cscale2 = [ [ float(i)/float(len(cl2)-1), cl2[i] ]
                for i in range(len(cl2)) ]

envelope4 = go.Scatter3d(
    name = 'Outer Singularities',
    x = envelope[:, 0],
    y = envelope[:, 1],
    z = envelope[:, 2],
    mode = 'markers',
    marker=dict(
        size=2,
        color = envelope[:, 2],
        colorscale = cscale1,
        opacity=0.6
    )
)
inner4 = go.Scatter3d(
    name = 'Inner Singularities',
    x = inner[:, 0],
    y = inner[:, 1],
    z = inner[:, 2],
    mode = 'markers',
    marker=dict(
        size=2,
        color = inner[:, 2],
        colorscale = cscale2,
        opacity=0.6
    )
)
layout = go.Layout(
    margin=dict(
        l=0,
        r=0,
        b=0,
        t=0
    )
)
data = [envelope4, inner4]
fig = go.Figure(data=data, layout=layout)
py.iplot(fig, filename='roof4_envelope')

References

[1] D. Haarhoff and S. Brell-Cokcan, “Vorrichtung zur Steuerung einer an einem Strang hängenden Last,”.

[2] D. Haarhoff, M. Kolditz, D. Abel, and S. Brell-Cokcan, “Actuator Design for Stabilizing Single Tendon Platforms,” in New Advances in Mechanisms, Mechanical Transmissions and Robotics: Proceedings of The Joint International Conference of the XII International Conference on Mechanisms and Mechanical Transmissions (MTM) and the XXIII International Conference on Robotics (Robotics ’16), vol. 46, B. Corves, E.-C. Lovasz, M. Hüsing, I. Maniu, and C. Gruescu, Eds. Springer International Publishing, 2016, pp. 371–378.

[3] Population Division - United Nations, “World Urbanization Prospects.” https://population.un.org/wup/. [Accessed: 23-May-2019]

[4] J. Morrison, “Construction Labor Shortage Creates Increasingly Lucrative Career Paths,” Forbes.. https://www.forbes.com/sites/jimmorrison/2018/08/05/construction-labor-shortage-creates-increasingly-lucrative-career-paths. [Accessed: 23-May-2019]

[5] D. M. Arntz, D. T. Gregory, and D. U. Zierahn, “Digitalisierung und die Zukunft der Arbeit: Makroökonomische Auswirkungen auf Beschäftigung, Arbeitslosigkeit und Löhne von morgen.” Zentrum für Euroäische Wirtschaftsforschung GmbH.

[6] T. Bock and T. Linner, Construction Robots: Elementary Technologies and Single-Task Construction Robots, vol. 3. Cambridge University Press, 2016.

[7] T. Bock and T. Linner, Robotic Industrialization: Automation and Robotic Technologies for Customized Component, Module, and Building Prefabrication. Cambridge University Press, 2015.

[8] T. Bock and T. Linner, Site Automation: Automated/Robotic On-Site Factories. Cambridge University Press, 2016.

[9] “SAM100 – Construction Robotics.”. https://www.construction-robotics.com/sam100/. [Accessed: 29-Aug-2020]

[10] “Rebar-Tying Robot | Pittsburgh, PA | TyBot, LLC,” tybot.. https://www.tybotllc.com. [Accessed: 29-Aug-2020]

[11] A. James, B. Roger, and D. Nicholas, “The NIST robocrane,” Journal of Robotic Systems, vol. 10, no. 5, pp. 709–724, 1993.

[12] A. M. Lytle, K. S. Saidi, W. C. Stone, and N. A. Scott, “Towards an Intelligent Job Site: Status of the NIST Automated Steel Construction Test Bed,” presented at the 20th International Symposium on Automation and Robotics in Construction, Eindhoven, The Netherlands, 2003, doi: 10.22260/ISARC2003/0036.

[13] K. Goodwin, “RoboCrane Construction of Bridges,” Transportation Research Record, vol. 1575, no. 1, pp. 42–46, Jan. 1997, doi: 10.3141/1575-06.

[14] A. Pott, H. Mütherich, W. Kraus, V. Schmidt, P. Miermeister, and A. Verl, “IPAnema: A family of Cable-Driven Parallel Robots for Industrial Applications,” Cable-Driven Parallel Robots, pp. 119–134, 2013, doi: 10.1007/978-3-642-31988-4_8.

[15] J. P. Sousa et al., “The SPIDERobot: A Cable-Robot System for On-site Construction in Architecture,” in Robotic Fabrication in Architecture, Art and Design 2016, D. Reinhardt, R. Saunders, and J. Burry, Eds. Cham: Springer International Publishing, 2016, pp. 230–239.

[16] J.-B. Izard et al., “Large-scale 3D printing with cable-driven parallel robots,” Constr Robot, vol. 1, no. 1, pp. 69–76, Dec. 2017, doi: 10.1007/s41693-017-0008-0.

[17] X. Weber, L. Cuvillon, and J. Gangloff, “Active vibration canceling of a cable-driven parallel robot using reaction wheels,” in 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2014, pp. 1724–1729, doi: 10.1109/IROS.2014.6942787.

[18] “HOME | ARCAS Project.”. http://www.arcas-project.eu/project-overview. [Accessed: 29-Aug-2020]

[19] Flight Assembled Architecture. 2011. https://vimeo.com/33713231. [Accessed: 23-May-2019]

[20] A. Mirjan, F. Augugliaro, R. D’Andrea, F. Gramazio, and M. Kohler, “Building a Bridge with Flying Robots,” in Robotic Fabrication in Architecture, Art and Design 2016, D. Reinhardt, R. Saunders, and J. Burry, Eds. Cham: Springer International Publishing, 2016, pp. 34–47.

[21] F. Augugliaro, A. Mirjan, F. Gramazio, M. Kohler, and R. D’Andrea, “Building tensile structures with flying machines,” in 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2013, pp. 3487–3492, doi: 10.1109/IROS.2013.6696853.

[22] J. Solly et al., “ICD/ITKE Research Pavilion 2016/2017: Integrative Design of a Composite Lattice Cantilever,” 16-Jul-2018.. https://www.ingentaconnect.com/content/iass/piass/2018/00002018/00000008/art00009. [Accessed: 23-May-2019]

[23] F. Inoue, K. Wiuanabe, Y. Ikeda, T. Wakisaka, A. Wakabayashi, and Y. Nekomoto, “A Practical Development of the Suspender Device that Controls Load Rotation by Gyroscopic Moments,” presented at the 15th International Symposium on Automation and Robotics in Construction, Munich, Germany, 1998, doi: 10.22260/ISARC1998/0036.

[24] S. Keating, N. A. Spielberg, J. Klein, and N. Oxman, “A Compound Arm Approach to Digital Construction,” in Robotic Fabrication in Architecture, Art and Design 2014, W. McGee and M. Ponce de Leon, Eds. Cham: Springer International Publishing, 2014, pp. 99–110.

[25] T. Burrell, A. Montazeri, S. Monk, and C. J. Taylor, “Feedback Control—Based Inverse Kinematics Solvers for a Nuclear Decommissioning Robot,” IFAC-PapersOnLine, vol. 49, no. 21, pp. 177–184, Jan. 2016, doi: 10.1016/j.ifacol.2016.10.541.

[26] F. A. Leve, B. J. Hamilton, and M. A. Peck, Spacecraft Momentum Control Systems. Springer International Publishing, 2015. https://www.springer.com/de/book/9783319225623. [Accessed: 04-Oct-2018]

[27] H. Kurokawa, “A Geometric Study of Single Gimbal Control Moment Gyros - Singularity Problem and Steering Law,” Agency of Industrial Technology and Science, Japan, 175, 1998.

[28] M. Oda, “Summary of NASDA’s ETS-VII robot satellite mission,” Journal of Robotics and Mechatronics, vol. 12, no. 4, pp. 417–424, Aug. 2000, doi: 10.20965/jrm.2000.p0417.

[29] K. Yoshida, K. Hashizume, and S. Abiko, “Zero reaction maneuver: flight validation with ETS-VII space robot and extension to kinematically redundant arm,” in Robotics and Automation, 2001. Proceedings 2001 ICRA. IEEE International Conference on, 2001, vol. 1, pp. 441–446.

[30] D. Reintsema, K. Landzettel, and G. Hirzinger, “DLR’s Advanced Telerobotic Concepts and Experiments for On-Orbit Servicing,” in Advances in Telerobotics, M. Ferre, M. Buss, R. Aracil, C. Melchiorri, and C. Balaguer, Eds. Berlin, Heidelberg: Springer Berlin Heidelberg, 2007, pp. 323–345.

[31] G. Hirzinger, B. Brunner, J. Dietrich, and J. Heindl, “ROTEX-the first remotely controlled robot in space,” in Proceedings of the 1994 IEEE International Conference on Robotics and Automation, 1994, pp. 2604–2611 vol.3, doi: 10.1109/ROBOT.1994.351121.

[32] Z. Li, H. Liu, and B. Wang, “Motion planning and coordination control of space robot using methods of calculated momentum,” in Robotics and Biomimetics (ROBIO), 2013 IEEE International Conference on, 2013, pp. 1151–1156.

[33] K. Yoshida, R. Kurazume, and Y. Umetani, “Dual arm coordination in space free-flying robot,” in Robotics and Automation, 1991. Proceedings., 1991 IEEE International Conference on, 1991, pp. 2516–2521.

[34] H. S. Jayakody, L. Shi, J. Katupitiya, and N. Kinkaid, “Robust Adaptive Coordination Controller for a Spacecraft Equipped with a Robotic Manipulator,” Journal of Guidance, Control, and Dynamics, vol. 39, no. 12, pp. 2699–2711, 2016, doi: 10.2514/1.G002145.

[35] L. Shi, J. Katupitiya, and N. Kinkaid, “A robust attitude controller for a spacecraft equipped with a robotic manipulator,” in 2016 American Control Conference (ACC), 2016, pp. 4966–4971, doi: 10.1109/ACC.2016.7526140.

[36] H. Wu, J. Qiao, Y. Chen, and Z. Li, “Robust Anti-Disturbance Coordination Control for Space Manipulator Systems with Multiple Disturbances,” in 2018 37th Chinese Control Conference (CCC), 2018, pp. 2601–2606, doi: 10.23919/ChiCC.2018.8483202.

[37] M. D. Carpenter, M. Peck, and others, “Reducing base reactions with gyroscopic actuation of space-robotic systems,” Robotics, IEEE Transactions on, vol. 25, no. 6, pp. 1262–1270, 2009.

[38] W. Xu, Z. Hu, Y. Zhang, and B. Liang, “On-orbit identifying the inertia parameters of space robotic systems using simple equivalent dynamics,” Acta Astronautica, vol. 132, pp. 131–142, Mar. 2017, doi: 10.1016/j.actaastro.2016.12.031.

[39] “JAXA | Engineering Test Satellite VII ‘KIKU-7’ (ETS-VII),” JAXA | Japan Aerospace Exploration Agency.. https://global.jaxa.jp/projects/sat/ets7/. [Accessed: 23-May-2019]

[40] G. Visentin and F. Didot, “Testing Space Robotics on the Japanese ETS-VII Satellite,” p. 5, 1999.

[41] P. P. Shīlovskīĭ, The gyroscope its practical construction and application, treating of the physics and experimental mechanics of the gyroscope, and explaining the method of its application to the stabilization of monorailways, ships, aeroplanes, marine guns, etc., London; New York: E. & F.N. Spon; Spon & Chamberlain, 1924. http://catalog.hathitrust.org/api/volumes/oclc/9511173.html. [Accessed: 21-May-2019]

[42] “The Rex and the Conte di Savoia.”. https://www.shippingwondersoftheworld.com/rex.html. [Accessed: 23-May-2019]

[43] D. K. Y. Kim, K. Bretney, A. Shao, and A. L. Tsang, “Electronic control system for gyroscopic stabilized vehicle,” US8532915B210-Sep-2013. https://patents.google.com/patent/US8532915B2/en. [Accessed: 10-Oct-2018]

[44] “Lit Motors.”. https://www.litmotors.com/. [Accessed: 29-Aug-2020]

[45] S. C. Spry and A. R. Girard, “Gyroscopic stabilisation of unstable vehicles: configurations, dynamics, and control,” Vehicle System Dynamics, vol. 46, no. sup1, pp. 247–260, Sep. 2008, doi: 10.1080/00423110801935863.

[46] “Seakeeper Gyro Stabilizers on Sealine Yachts - YouTube.”. https://www.youtube.com/watch?v=f4vZtk1jZ5Q. [Accessed: 23-May-2019]

[47] J. D. Adams and S. W. McKenney, “Gyroscopic roll stabilizer for boats,” US6973847B213-Dec-2005. https://patents.google.com/patent/US6973847B2/en?assignee=seakeeper&oq=seakeeper. [Accessed: 10-Oct-2018]

[48] J. D. Adams and S. W. McKenney, “Cooling bearings, motors and other rotating heat generating components,” US7546782B216-Jun-2009. https://patents.google.com/patent/US7546782B2/en?assignee=seakeeper&oq=seakeeper. [Accessed: 10-Oct-2018]

[49] J. Gagne, O. Piccin, E. Laroche, and J. Gangloff, “A cardiac motion compensation device based on gyroscopic effect,” IFAC Proceedings Volumes, vol. 42, no. 16, pp. 505–510, Jan. 2009, doi: 10.3182/20090909-4-JP-2010.00086.

[50] J. Gagne, O. Piccin, É. Laroche, M. Diana, and J. Gangloff, “GyroLock: Stabilizing the Heart With Control Moment Gyroscope (CMG)—From Concept to First In Vivo Assessments,” IEEE Transactions on Robotics, vol. 28, no. 4, pp. 942–954, Aug. 2012, doi: 10.1109/TRO.2012.2188162.

[51] J. Chiu and A. Goswami, “Design of a Wearable Scissored-Pair Control Moment Gyroscope (SP-CMG) for Human Balance Assist,” p. V05AT08A023, Aug. 2014, doi: 10.1115/DETC2014-35539.

[52] D. Li and H. Vallery, “Gyroscopic assistance for human balance,” in 2012 12th IEEE International Workshop on Advanced Motion Control (AMC), 2012, pp. 1–6, doi: 10.1109/AMC.2012.6197144.

[53] B. Thornton, T. Ura, Y. Nose, and S. Turnock, “Internal actuation of underwater robots using control moment gyros,” in Oceans 2005-Europe, 2005, vol. 1, pp. 591–598.

[54] B. Thornton, T. Ura, Y. Nose, and S. Turnock, “Zero-G Class Underwater Robots: Unrestricted Attitude Control Using Control Moment Gyros,” IEEE Journal of Oceanic Engineering, vol. 32, no. 3, pp. 565–583, Jul. 2007, doi: 10.1109/JOE.2007.899274.

[55] E. Yime, H. Moreno, R. Saltarén, and R. Aracil, “Design of a CMG for underwater robots,” in OCEANS 2011 IEEE-Spain, 2011.

[56] C. Peña, R. Aracil, R. Saltaren, E. Yime, P. Cardenas, and J. Quintero, “Advances in developing telemanipulators for an underwater robot-Remo II,” in OCEANS 2009-EUROPE, 2009, pp. 1–8.

[57] J. W. Romanishin, K. Gilpin, and D. Rus, “M-blocks: Momentum-driven, magnetic modular robots,” in Intelligent Robots and Systems (IROS), 2013 IEEE/RSJ International Conference on, 2013, pp. 4288–4295.

[58] M. Gajamohan, M. Merz, I. Thommen, and R. D’Andrea, “The Cubli: A cube that can jump up and balance,” in 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2012, pp. 3722–3727, doi: 10.1109/IROS.2012.6385896.

[59] H. Kanki, Y. Nekomoto, H. Monobe, H. Ogura, and K. Kobayashi, “Development of CMG Active Vibration Control Device for Gondola,” JSME international journal. Ser. C, Dynamics, control, robotics, design and manufacturing, vol. 37, no. 3, pp. 468–470, Sep. 1994, doi: 10.1299/jsmec1993.37.468.

[60] 김석호, 정삼용, and 손양현, “Hook block of possible rotary adjusting,” KR970003509B118-Mar-1997. https://patents.google.com/patent/KR970003509B1/en?q=Hook&q=Block&q=Possible&q=Rotary&q=Adjusting&oq=Hook+Block+of+Possible+Rotary+Adjusting. [Accessed: 22-Jan-2019]

[61] K. Y. Yi and Y. G. Chung, “An implementation of a gyro actuator for the attitude control of an unstructured object,” in Proceedings 1999 IEEE International Conference on Robotics and Automation (Cat. No.99CH36288C), 1999, vol. 2, pp. 1626–1631 vol.2, doi: 10.1109/ROBOT.1999.772592.

[62] K. Y. Yi and M.-O. Gwon, “An Attitude Control and Stabilization of an Unstructured Object using CMG Subsystem,” The Transactions of the Korean Institute of Electrical Engineers D, vol. 49, no. 8, pp. 459–466, 2000. http://www.koreascience.or.kr/article/JAKO200011919588608.page. [Accessed: 08-Feb-2019]

[63] Y. H. Sa, K. Y. Yi, and J.-O. Kim, “An attitude control and stabilization of an unstructured object using balancing beam, new construction machinery,” in ISIE 2001. 2001 IEEE International Symposium on Industrial Electronics Proceedings (Cat. No.01TH8570), 2001, vol. 2, pp. 792–797 vol.2, doi: 10.1109/ISIE.2001.931569.

[64] C. Lee, G. Lee, S. Park, and J. Cho, “Analysis of field applicability of the rotation-controllable tower-crane hook block,” Automation in Construction, vol. 21, pp. 81–88, Jan. 2012, doi: 10.1016/j.autcon.2011.05.015.

[65] N. Kodani, S. Ouchi, and K. Takahashi, “Transporting and rotating control of a jib crane by control moment gyro,” in IECON 2017 - 43rd Annual Conference of the IEEE Industrial Electronics Society, 2017, pp. 8297–8302, doi: 10.1109/IECON.2017.8217456.

[66] S. THOMSON and A. Miller, “Materials management systems and methods,” WO2017059493A113-Apr-2017. https://patents.google.com/patent/WO2017059493A1/en?oq=verton. [Accessed: 21-Sep-2019]

[67] E. M. Abdel-Rahman, A. H. Nayfeh, and Z. N. Masoud, “Dynamics and control of cranes: A review,” Modal Analysis, vol. 9, no. 7, pp. 863–908, 2003. https://journals.sagepub.com/doi/pdf/10.1177/1077546303009007007

[68] C. Hill, Learning Scientific Programming with Python, 1st ed. Cambridge University Press, 2016.

[69] W. O’Connor and H. Habibi, “Gantry crane control of a double-pendulum, distributed-mass load, using mechanical wave concepts,” Mechanical Sciences, vol. 4, no. 2, pp. 251–261, Jul. 2013, doi: https://doi.org/10.5194/ms-4-251-2013.

[70] SWF Krantechnik - Lastpendeldämpfung - Sway Control. 2012. https://www.youtube.com/watch?v=Wid0DKa2HtY. [Accessed: 29-Jul-2019]

[71] W. Singhose and D. Kim, “Manipulation with Tower Cranes Exhibiting Double-Pendulum Oscillations,” in Proceedings 2007 IEEE International Conference on Robotics and Automation, 2007, pp. 4550–4555, doi: 10.1109/ROBOT.2007.364180.

[72] W. Singhose, D. Kim, and M. Kenison, “Input Shaping Control of Double-Pendulum Bridge Crane Oscillations,” Journal of Dynamic Systems, Measurement, and Control, vol. 130, no. 3, p. 034504, 2008, doi: 10.1115/1.2907363.

[73] J. Vaughan, D. Kim, and W. Singhose, “Control of Tower Cranes With Double-Pendulum Payload Dynamics,” IEEE Transactions on Control Systems Technology, vol. 18, no. 6, pp. 1345–1358, Nov. 2010, doi: 10.1109/TCST.2010.2040178.

[74] D. Kim and W. Singhose, “Performance studies of human operators driving double-pendulum bridge cranes,” Control Engineering Practice, vol. 18, no. 6, pp. 567–576, Jun. 2010, doi: 10.1016/j.conengprac.2010.01.011.

[75] A. M. Abdullahi et al., “Adaptive output-based command shaping for sway control of a 3D overhead crane with payload hoisting and wind disturbance,” Mechanical Systems and Signal Processing, vol. 98, pp. 157–172, Jan. 2018, doi: 10.1016/j.ymssp.2017.04.034.

[76] N. Sun, Y. Wu, Y. Fang, and H. Chen, “Nonlinear Antiswing Control for Crane Systems with Double-Pendulum Swing Effects and Uncertain Parameters: Design and Experiments,” IEEE Transactions on Automation Science and Engineering, vol. 15, no. 3, pp. 1413–1422, Jul. 2018, doi: 10.1109/TASE.2017.2723539.

[77] N. Q. Hoang, S.-G. Lee, J.-J. Kim, and B. S. Kim, “Simple energy-based controller for a class of underactuated mechanical systems,” Int. J. Precis. Eng. Manuf., vol. 15, no. 8, pp. 1529–1536, Aug. 2014, doi: 10.1007/s12541-014-0501-z.

[78] I.-S. Won, “Comparative study of energy-based control design for overhead cranes,” International Robotics & Automation Journal, vol. 4, no. 3, p. 7.

[79] S.-G. Lee and N. Hoang, “Energy-Based Approach for Controller Design of Overhead Cranes: A Comparative Study,” Applied Mechanics and Materials, vol. 365, pp. 784–787, Jun. 2013, doi: 10.4028/www.scientific.net/AMM.365-366.784.

[80] M. K. Ozgoren, “Comparative study of attitude control methods based on Euler angles, quaternions, angle–axis pairs and orientation matrices,” Transactions of the Institute of Measurement and Control, vol. 41, no. 5, pp. 1189–1206, Mar. 2019, doi: 10.1177/0142331218773519.

[81] “What are singularities in a six-axis robot arm?” https://www.mecademic.com/resources/Singularities/Robot-singularities. [Accessed: 12-Aug-2020]