MAZE TAVERSING SYSTEM Proposal for ECSE-4962 Control System Design

Transcription

1 MAZE TAVERSING SYSTEM Proposal for ECSE-4962 Control System Design Rich Pingelski Tushar Shah Mark Stenpeck Ian Trettel February 22, 2006 Rensselaer Polytechnic Institute

2 Abstract The goal of this project is to design a ball on plate system capable of traversing a steel ball through a predefined maze. Given sufficient time and resources we would like to replicate the processes preformed by a human to accomplish the task as near as possible using a computer controlled mechanical system. By adding a joystick, comparison between a human and computer reactions could help to form a model of how the brain handles fine motor control. The impact this research would have on those in our society who have prosthetic limbs would be enormous, allowing engineers for the first time the ability to construct a prosthesis that acts naturally. ii

3 Table of Contents 1 INTRODUCTION OBJECTIVE PROJECT GOALS SOCIAL IMPLICATIONS DESIGN STRATEGY MODEL DEVELOPMENT Inertia Matrices Dynamic Model Simulation Controller Design and Tuning PERFORMANCE SPECIFICATIONS VS. AVAILABLE COMPONENTS Mechanical Electrical PARAMETER IDENTIFICATION SUBSYSTEM DEVELOPMENT Resistive Touch Screen Inclinometer Vision System (Optional) DESIGN ALTERNATIVES PLAN OF ACTION MODEL DEVELOPMENT PARAMETER IDENTIFICATION CONTROLLER DESIGN SYSTEM INTEGRATION BASIC SYSTEM ADVANCED SYSTEM VISION SYSTEM (OPTION) VERIFICATION TESTING PROCEDURE TOLERANCE ANALYSIS COST AND SCHEDULE COST ANALYSIS SCHEDULE BIBLIOGRAPHY STATEMENT OF CONTRIBUTION APPENDIX iii

4 List of Tables TABLE 6.1 PARTS AND ASSOCIATED COSTS TABLE 6.2 MATERIALS AND ASSOCIATED COSTS TABLE 6.3 LABOR COSTS TABLE 6.4 SCHEDULE iv

5 List of Figures FIGURE 3.1 BALL DYNAMICS... 4 FIGURE 3.2 CONTROL MODEL... 4 v

6 List of Appendixes APPENDIX A RESISTIVE TOUCH SCREEN APPENDIX B PAN-TILT_MODELVIEW APPENDIX C PAN-TILT_MODELVIEW (WITHOUT BASE) APPENDIX D MASS PROPERTIES OF PANTILT APPENDIX E TEST MAZE APPENDIX F PANTILT.M vi

7 1 Introduction The goal of this project is to design a ball on plate system capable of traversing a steel ball through a predefined maze. To this end a device will be constructed which will allow for the creation of rotational movement about two horizontal axes. Each of these axes will be controlled via an independent motor coupled with software and position feedback. The position of the ball relative to the plate will be monitored by a resistive touch screen device. Similar systems have been created in the past that display some of the same features as this proposal. In the spring of 2004 a team of students in Control Systems Design created a system wherein a steel ball was balanced on a plate. Their project ended in partial success, as the ball was indeed able to be balanced and achieve controlled straight line motion, but non-linear routes proved much more difficult and ultimately unattainable. Another similar project has been developed here at RPI by a team of engineers lead by Professor Kevin Craig of the Mechanical, Aerospace and Nuclear Engineering department. They have developed a system involving a ball on plate balancing device which is capable of balancing the ball at any point on the plate, as well as tracing complex paths. This project intends to improve upon these previous designs by incorporating the tracing of paths through a maze, as well as overall improvements upon the control design of these previous systems. Specifically we will design a system capable of placing the ball within ±10mm/m (1mm/10cm) of a desired location. This level of accuracy is demanded by the constraints of the maze environment and consequently will require the design of controllers which have fast rise times, low overshoot, minimal settling times, and no steady state error. 1

8 2 Objective 2.1 Project Goals The initial goal of the proposed system would be to traverse straight line paths in a maze corridor without significant deviations from the desired path. Building upon this, turns in the maze corridor would then be inserted, forcing the ball to round corners and correctly negotiate four way intersections. Finally, non-linear paths would be included to assess system performance in a situation of fully coupled horizontal axes. Several challenges will arise in the design and implementation of the system. The first, and perhaps most severe of which is the very fine motor control that will be required to accurately position and move the ball through the maze. Compounding this, the resolution of the encoder wheel motor feedback system on one axis is ±.35, which could prove to be too coarse of a measurement and impact the ability of the system to level properly. Also challenging will be procurement of a material which has a sufficiently high coefficient of friction which will not impact the sensitivity of the resistive touch pad. This is critical in that the ball can not be allowed to slide across the plate as this will impact control performance, so the coefficient of friction must be high. However if the material is too thick it will adversely affect the performance of the resistive touch pad leading to a situation where position data from the ball is unavailable to the controller. Another challenge is in the design of the control system which will govern the system response. The design of controllers which possess fast, accurate response is not a trivial matter and we anticipate significant time spent on designing these controllers. Currently we are pursing a controller consisting of two loops, the outer relating to the ball position on the plate, and the inner, the torque generated by the control motors. 2.2 Social Implications A project such as this has various important implications for our society. One of which is the possibility of coupling this project with a cognitive psychology research project whose aim is to map the control algorithm the human brain uses to control the arm. Modification of the system for such a project would be simplistic and involve only adding a joystick and software which logs both ball position and joystick inputs. The impact this research would have on those in our society who have prosthetic limbs would be enormous, allowing engineers for the first time the ability to construct a prosthesis that acts naturally. 2

9 3 Design Strategy 3.1 Model Development Inertia Matrices In developing our physical model our first step was to take the inertia matrices for the two links from Solid Works. Those matrices are in units (g*cm 2 ): I a 1.00 = I b 0.98 = To develop a complete non-linear model of our system, we have made use of the provided pantilt.m matlab code. This code assumes that the system is setup with one link rotating about the y axis and another about the z axis. Our system however has one rotating about the y axis and one about the x axis. The code was easily modified to reflect this by changing the matrix h1 to be equal to x as apposed to z (Appendix F). From this code we were able to obtain the matrices for Mass/Inertia (M), Corriolis effects (C) and Gravity loading (G). The adjustment to the code also results in a coupling between the axes in the gravity term. This provides us with the majority of the terms for the dynamic model of the arm. The only term left to determine is the friction term (B). This term can be obtained through experimental data once the system is constructed... M ( θ ) θ + B( θ ) + C( θ, θ ) θ + G( θ ) = τ Dynamic Model There is a second set of dynamics which must be considered when modeling our system. These are the dynamics of the ball that will be rolling within our maze. The modeling of this is easier than the dynamics of the system as a whole. Since for this part of the system we can assume complete decoupling, it can be modeled in on each axis direction by a ball rolling down a beam. The forces present are those of gravity, inertia and friction as follows: 3

10 Figure 3.1 Ball Dynamics Friction = B x+ K sgn( x) Gravity = mg sin( θ ).. Inertia = m x.. Sum _ of _ Forces :.. mg sin( θ ) = m x+ B x+ K sgn( x) The same equations are true in the y axis by replacing x with y. The constants are all known. B&K will be based on the coefficient of friction of the rubber padding and m is the mass of the ball Simulation To simulate our system we built a model in Matlab s Simulink program. Figure 3.2 Control Model For the moment this simulation appears hollow as our controller has not taken form yet. We have decided on a PID controller for both the arm controller and the ball 4

11 controller. Once we have experimental data to determine the missing constants in the dynamic equations, we can fill in the constant gains of the controllers. We can draw some important information from this diagram. For each axis there are 2 control loops to be considered. The first is the outer loop which takes in the desired position of the ball and moves the ball towards that point. The second is the inner loop which takes in the ever changing desired angle to move the ball towards its desired position. The points marked Theta Actual and X actual are return data points from the encoders and touchpad sensor respectively. This feedback will provide smoother control Controller Design and Tuning From the development of our model we believe we can still implement our system using a PID controller. Our state variable vector remains unchanged: x = xm. x m ym. y m θ p. θ p θ t. θ t Our first milestone of moving the ball down a linear corridor and having it stop without relying on the walls of the corridor is definitely attainable through a PID type controller with adequate disturbance rejection. The actual proportional, integral and derivative constants will be determined experimentally once the physical system is built. Also the controller must be linearized to make its implementation practical. To do this we will assume that angular velocities of the pan and tilt axes are small enough to ignore. Therefore we will have allowed the model to operate around a small, almost linear region centered about some Ө d. The form of the controller is: D( z) = K Where T is the sampling period. p + K I T z K 2 z 1 D z 1 Tz 5

12 3.2 Performance Specifications vs. Available Components Mechanical This project will require many fundamental changes to the assigned system from last year, but these modifications are surmountable. The basic design of the system requires two tilt axes, while we were given one pan and one tilt. This will result in the entire system being disassembled and reconfigured. We can, though, reuse many of the critical parts from the given system, reducing our manufacturing load. From last year s system, we can reuse the main support brackets and their associated motor mounts. A new base, fitting our dimensions, will have to be created, along with a bracket to hold the plate, and a yoke to surround it. The SolidWorks model is shown in Appendix B. Based on our calculations, motor torque will have to be maximized to account for the loading of the system. The primary axis of motion will have to move the ball, plate, its support, and the motor/gear assembly of the secondary axis. Accounting for all of this gives an inertial mass of 2mN m, well within the specifications of 43mN m continuous torque. Maximum instantaneous torque was calculated to be 0.059N m on the principal axis and 0.07N m on the secondary axis. Using gear ratios of 4:1 on both axes, we will have a maximum torque provided by the motors of N m, giving us a factor of safety of Currently, the pan axis only has a 2:1 gear ratio. While this is sufficient to run the secondary axis, it only provides a factor of safety of 1.14, which this team believes to be too close to the minimum value, and therefore would need to update the gear train to accommodate this Electrical Another aspect that must be considered is the encoder resolution of the given system. We have on hand one 2048-level encoder, and one 1024-level encoder. Being that the plate will only be rotating a maximum of ±10º from level in either direction, the 1024-level encoder is only capable of 0.4º /division. A rough estimation of the rolling coefficient of friction of rubber on steel was made to be 0.01 [1], and the actual value could be much lower. This causes the minimum angle for ball movement to be 0.57º. A 0.4º/division encoder will numerically work, but does not allow for a large factor of safety nor account for the inherent inaccuracies of our work. Having a 2048-level encoder will remove many of the error-prone steps in the process, and simplify the mechanical build by orders of magnitude. Past projects of this caliber [2] have cited a problem with A/D converter resolution. Currently, the controller chip (3M model SC801U USB Controller) has a 10 bit converter built in, allowing for a 1024x1024 resolution size. We propose to use the LabView software s built in ADC, which is programmable from bits, mitigating any resolution issue. The pad operates on 5V, so with 10 bits of resolution there is a difference of 5mV in each individual position. If we were to use 16 bit conversions, there 6

13 would be a 76μV difference. If proven possible, we would like to operate the touchpad at a higher voltage, thereby increasing the difference in digital levels and decreasing our positioning error. Another critical component to decreasing error would be to minimize noise in the system by issuing a quality control point that all connections from the touchpad to the system are as short as possible, and contain no loops or unnecessary windings in the cord to prevent any induction or other sources of noise corrupting the lines. The touchpad itself will be covered with a 1/16 latex rubber sheet [3], to prevent the ball from slipping as it rolls, further complicating the ball dynamics. This will decrease the sensitivity of the touchpad. With the pad, a pressure of 60g point force was enough to excite the pad. This is the equivalent of a 1.0 diameter steel ball. The final item that must be added to the system is some initial calibration routine. In order to let the controller know that the plate is level to start, there needs to be some method of feedback relative to the ground. One method of such is an inclinometer. The exorbitant cost of these precision instruments, though, means that it is not a feasible means of leveling for this project. 3.3 Parameter Identification The model for the system will contain a number of parameters, each having varying degrees of knowledge at this time. We have four input parameters: Current tilt angle of both axes. Current position of ball on both axes. And two output parameters: Desired tilt angles of both axes. Inside the model, there are a number of forces that have to be accounted for. These are: Masses of the ball, plate, secondary axis motor and frame Moments and Inertial masses of aforementioned objects Frictional Forces o Rolling friction of the ball on rubber pad. o Internal friction of motor. o Shaft rotation of primary and secondary axes of rotation. o Static friction between the belts and the gears. Frictional losses will be lumped together: determining each experimentally would be an inordinate use of our limited time, and since they can all be combined using the principle of superposition, there exists little reason not to lump the forces. This identification will be done by forcing the motor to a constant torque and measuring the resulting velocity. The difference between this result and the torque-speed graph will be the frictional losses of the system. Once the system is constructed this measurement will occur. 7

14 3.4 Subsystem Development Resistive Touch Screen Knowing the current location of the ball within the maze is the crucial factor in reaching the objective of navigating a ball from one point to another. To this end a resistive touch screen has been chosen based on the experiences of past projects. Pressure applied to the screen changes its resistive property modifying an analog voltage read out. The voltage read out is converted to (X, Y) coordinates. An 8-pin female connector is normally supplied with the screen but had been removed from the system we received. A description of the control chip is included in Section Inclinometer In order to provide consistent results, the touch screen surface (acting as the maze floor) should be leveled at the beginning of each session. When properly leveled, the ball will not roll in any direction when placed within the maze. Additionally the maze walls should not be used to prevent the ball from rolling at initial condition. This can be achieved by adding inclinometers to the X and Y tilt axles. An inclinometer is a digital tilt sensor used to report the angle of an object with respect to gravity. The system would be calibrated at start-up with a zero position established for each axle. The inclinometer s output signal interfaces directly to the A/D converter of the computer system. The Matlab code would read the digital signal and calibrate the system at start-up Vision System (Optional) The stated goal of this project is to traverse a ball through a predefined maze using a program written specifically for that particular maze. Should time and resources permit we would like to implement a visual recognition system that when shown a maze can distinguish between the walls and floor. A means of indicating where the ball will start and finish in the maze would also need to be addressed, possible by touching the screen. Using this information an algorithm would be developed to determine the best path. When the ball is placed at the starting location the mechanical control system would then move the ball to the finish. The ultimate goal would be to only require indication of the finish location and when the ball is placed at any location in the maze the controller do the rest. This would require the touch screen sensing the current ball location, relating it to the maze corridors, and finally determining a path. To do this would simulate all the thought processes of the human mind with the exception of using feel to locate the ball instead of vision. 8

15 3.5 Design Alternatives A second, less accurate method of establishing initial leveling conditions would be to put pressure switches on opposite corners of the base, and create a calibration routine to rotate both axes until these switches are hit. The issue with this is that the table is only guaranteed to be parallel to the base, and not necessarily level to the floor. If the base can be guaranteed to be parallel, this method is acceptable, but only in that case. Its advantage is cost: switches are inexpensive. The current proposal calls for a completely new system to be constructed by the team. In a conversation with Kevin Craig, Professor of Mechanical Engineering/ Mechatronics at the Rensselaer Polytechnic Institute, we were offered use of his ball-onplate system for our project. This alleviates any of the physical build problems that would have been encountered in our own work on a system, and allows us to focus our efforts on parameter identification and controller design/tuning. We are currently waiting on contact from Prof. Craig so that we can get access to the system and have a close look at the parameters (motor power, controller interfacing, amplifiers) so that we may evaluate the feasibility of implementing this system. We believe that the system will successfully interface with our controllers without major hardware changes. Possible changes include: Amplifiers that match the ones located in the CSD lab. New cabling to connect the system to the NI crio controller. Replacing the existing encodes with ones compatible with the crio modules. Addition of bits to the hardware A/D converter that is currently included with the resistive touchpad. It is believed to only be 8 bits wide, which is woefully insufficient for our control. Problems that would preclude the use of Professor Craig s system: Motors that require more power than we are able to supply. Encoders that are not compatible with our controller, and cannot be replaced with our encoders. It would be possible to change the motors of the system, but this would require major mechanical changes to the system, making it uneconomical to use the Mechanical Engineering Department s system. 9

16 4 Plan of Action The development of our system will consist of six distinct milestones delineating the achievement of major system progress or functionality goals. These six milestones are described in detail below. 4.1 Model Development SolidWorks Model Develop an accurate and complete system model in SolidWorks to obtain system mass and inertia information. Dynamic System Model Develop a mathematical model of the fully coupled system. Simulink Model Develop a dynamic system model in Simulink for accurate, time based, system modeling and model verification. 4.2 Parameter Identification Motor Parameters Determine internal motor friction, both coulomb and vicious, as well as developing a speed / torque curve. Link Parameters Determine link friction at the bearings for inclusion into the dynamic system model. 4.3 Controller Design Controller Development & Simulation Develop controllers which tune the system response to within the desired specifications. Simulate these controllers with the system model in Simulink and asses total system response. Controller Tuning Tune controllers for optimum response. 10

17 4.4 System Integration LabView Implementation Develop LabView code which integrates controllers into the physical system, as well as code which interfaces with the systems motors and feedback systems. Verify Response Verify system response and tune both the controllers and system model if necessary. 4.5 Basic System Straight Line Corridor Move a ball through a straight line corridor of the maze, then bring the ball to a stop before hitting a wall. Disturbance Rejection Verify the system has adequate disturbance rejection by moving the ball to a nonoptimal location and observing the system response. 4.6 Advanced System Simple Maze Move the ball through a simple maze consisting of 2 basic turns. Advanced Maze Move the ball through an advanced maze consisting of several turns and at least one 4 way intersection. Non-Linear Maze Move the ball through a maze consisting of non-linear paths. 4.7 Vision System (option) Develop a vision system capable of mapping a maze and instructing the system how to move the ball through it. 11

18 5 Verification 5.1 Testing Procedure The mechanical system will be tested first for alignment and free movement. As long as the parts are machined to tolerance there shouldn t be any issues. With both tilt motors de-energized the mounting plate should rotate smoothly. To fine tune the balancing of the mechanical system, a counterweight will be added to offset the secondary tilt motor attached to the yolk. After the counterweight is installed the mounting frame should not rotate when the motors are de-energized. The motors ability to rotate the mounting plate will be tested next. By applying different inputs signals to the motors separately, Simulink and Matlab can be used to graph the rotational position response from the optical encoder. Based on the results the decision will be made whether to increase or decrease the motor torque requirement. To roll the ball smoothly the responses of both motors must be similar. An addition test will be performed to measure the response while the plate is rotated diagonally using both motors in concert. Several tests must be performed on the touch screen subsystem. Calibrated masses were used to test the force required to sensitize the touch screen. Confirmation of the ball s weight to register on the screen is needed. A smaller diameter ball is prefers because the active screen area is limited so the complexity of the maze is effected. The ball s traction on the friction cover preventing sliding will also be observed. With the touch screen tilted at an extreme angle the ball should roll when released. If the ball slides at all a material with a higher frictional coefficient will be required. During the testing of the ball s size the screen resolution will also be addressed. In order to provide accurate positional feedback the screen resolution must be adequate. If during testing it doesn t meet our requirements steps must be taken to either reduce excessive noise or increase the sampling time. After all the mechanical and sensing questions have been addressed the control algorithms will be tested. Simple tasks like starting the ball rolling and stopping at a certain point along a straight corridor of a test maze will be performed first. When those tasks can be performed repeatedly rounding a corner will be attempted. With the computer program now able to direct the ball in two directions the most complicated maneuvers can be tested. This includes doing U-turns and passing through 4-way intersections. Finally running a predefined pattern can be implemented and tuned to traverse the maze faster. 12

19 5.2 Tolerance Analysis The parameter with the greatest effect on the system s overall performance is friction. The limit of rotational motion of the plate is only a few degrees. Most of the controller corrections will be small incremental angular movements by the DC gear motors. Each of these movements requires overcoming static friction which is extremely hard to model mathematically. Many current control systems treat static friction as an impulse. We can not take this approach because excessive overshoot can cause the ball to jump and skid before rolling. In order to maneuver through the maze with the highest precision the velocity of the ball s center of mass must be determined continuously. A feed-forward algorithm is generally used to control small movements. Our intention is to treat friction as a disturbance. There are a number of mechanical factors contributing to the total friction in the system so we are unable to rely solely on the computer simulation. Graphically analysis of experimental data received from testing on the mechanical system should provide a more reliable parameter. As previously mentioned in Section 3.3 the relationship between torque and speed will be utilized. 13

20 6 Cost and Schedule 6.1 Cost Analysis The total cost for our system consists of the individual costs for labor, parts, and materials. This breakdown is detailed in the tables below. Table 6.1 Parts and Associated Costs Item # Supplier Unit Cost Total Cost 1 Dia. Bearing Ball 1 Donated $0.00 $0.00 Pitman GM9234S017 2 Supplied $97.59 $ US Digital S B 1 Supplied $60.71 $60.71 US Digital S B 1 Supplied $60.71 $60.71 US Digital T $85.50 $ Lexan 1 Donated $0.00 $0.00 1/16 Thk. Latex Rubber Pad 1 McMaster $13.14 $13.14 Bearings 4 Igus $0.00 $0.00 Total Cost $ Table 6.2 Materials and Associated Costs Item # Unit Cost Total Cost Supplier Balsa wood $ cm aluminum round stock $4/lb RPI Machine Shop 4cm aluminum stock $4/lb RPI Machine Shop Total Cost Table 6.3 Labor Costs Engineer Hourly Wage Total Hours Total Pingelski, Richard $ $9,000 Shah, Tushar $ $9,000 Stenpeck, Mark $ $9,000 Trettel, Ian $ $9,000 Total Cost $36,000 14

21 6.2 Schedule To achieve these project goals the following schedule has been developed to aid in the allocation of tasks related to project milestones. Table 6.4 Schedule Week Task Responsible Party 4 SolidWorks CAD Model Ian Dynamic System Model Tushar SolidWorks CAD Model Ian 5 Parameter ID Rich, Mark Simulink Model Tushar 6 Initial Controller Development Rich, Ian Model Validation Tushar, Mark Motor / Encoder Interface Development Ian, Mark 7 System Simulation Tushar Controller Tuning Rich 8 LabView Controller Development Ian System Response Verification Team 9 Basic System - Straight Line Path Team 10 Advanced System Simple Maze Team 11 Advanced System Advanced Maze Team 12 Advanced System Non-Linear Maze Team 13 Final Report Development Team Final Presentation Development Team 14 Final Report Development Team Final Presentation Team 15 Final Report Team 15

22 7 Bibliography [1] Rolling Friction Succeed in Physical Science. Online. Internet. Accessed 20 February Available [2] Ball on Plate Balancing System. G. Andrews, C. Colasuonno, A. Herrmann. Rensselaer Polytechnic Institute, Unpublished, available: [3] Rubber and Foam. Online. Internet. Available: 16

23 8 Statement of Contribution Rich Pingelski completed: Edited Proposal Subsystem Development Testing Procedures Tolerance Analysis Tushar Shah completed: Inertia Matrices Dynamic model Simulation Controller Design and Tuning Mark Stenpeck completed: Performance Specification vs. Available Components Parameter Identification Design alternatives Ian Trettel completed: Introduction Objective Plan of action Cost and Schedule SolidWorks Rich Pingelski Tushar Shah Mark Stenpeck Ian Trettel 17

24 9 Appendix Appendix A Resistive Touch Screen 18

25 Appendix B Pan-tilt_modelview No belts shown. 19

26 Appendix C Pan-tilt_modelview (without base) This sub-assembly is the portion of the overall mechanism that will be rotating along two axes. Counterweight will be added to offset the motor mounted on the frame. 20

27 Appendix D Mass Properties of PanTilt2 21

28 Appendix E Test Maze 22

29 Appendix F Pantilt.m *** M: Mass/Inerita Matrix *** M_11 (mb*lcb3^2-ib33-mb*lcb1^2+ib11)*c2^2+ (-2*s2*mB*lcB1*lcB3+2*s2*IB13)*c2+IA33+mA*lcA1^2+mA*lcA2^2+IB33+mB*lcB1^2+mB*lcB2^2 M_12 (IB23-mB*lcB2*lcB3)*s2+c2*IB12-c2*mB*lcB1*lcB2 M_21 c2*ib12-c2*mb*lcb1*lcb2+s2*ib23-s2*mb*lcb2*lcb3 M_22 IB22+mB*lcB1^2+mB*lcB3^2+Im2*N2^2 *** C: Coriolis/Centrifugal Term *** -(2*c2*IB11*s2+2*c2*mB*lcB3^2*s2+2*IB13-4*c2^2*IB13-2*mB*lcB1*lcB3+4*c2^2*mB*lcB1*lcB3-2*IB33*c2*s2-2*mB*lcB1^2*c2*s2)*td2*td1-(td2*s2*IB12-td2*s2*mB*lcB1*lcB2- td2*c2*ib23+td2*c2*mb*lcb2*lcb3)*td2 (c2*ib11*s2+c2*mb*lcb3^2*s2+ib13-2*c2^2*ib13-mb*lcb1*lcb3+2*c2^2*mb*lcb1*lcb3- IB33*c2*s2-mB*lcB1^2*c2*s2)*td1^2 *** G: Gravity Term *** ma*g*c1*lca2-ma*g*s1*lca3+mb*g*s1*s2*lcb1+mb*g*c1*lcb2-mb*g*s1*c2*lcb3 -mb*g*c1*c2*lcb1-mb*g*c1*s2*lcb3 23