kinematic, dynamic and workspace analysis of a novel 6 …€¦ · i also thank the armlab members...

KINEMATIC, DYNAMIC AND WORKSPACE ANALYSIS OF A NOVEL 6-DOF PARALLEL MANIPULATOR

by

Hrishi L. Shah

Aug 11, 2010

A thesis submitted to the

Faculty of the Graduate School of

the University at Buffalo, State University of New York

in partial fulfillment of the requirements for the

degree of

Master of Science

Department of Mechanical and Aerospace Engineering

ii | P a g e

8/11/2010

MAE 560 - KRO Hrishi Shah

SUNY BUFFALO

KINEMATIC, DYNAMIC AND WORKSPACE ANALYSIS OF A NOVEL 6-DOF PARALLEL MANIPULATOR

iii | P a g e

To my parents and friends.

You complete me.

iv | P a g e

Acknowledgement

I express my sincerest gratitude for my advisor, Dr. Krovi, for the constant guidance and support

throughout the course of my MS degree, especially during the thesis.

I also express my thanks to Dr. Roger Mayne and Dr. John Crassidis for being my committee members

and critiquing my thesis report.

I also thank the ARMLAB members for their support. Special thanks goes to LengFeng and Madu for

helping to write the publications related to this thesis. I would also like to thank Sumit for being an excellent

labmate, roommate and project partner and for his critique to my ideas and concepts throughout the duration of

the thesis. I am also grateful to Xiaobo and Colin for being helpful and patient whenever I needed their help.

I thank my friends in Buffalo, Vivek, Suresh, Vinay, for making this stay comfortable, enjoyable and

productive. I also thank Dr. and Mrs. Dhiraj Shah for helping continually and graciously during my entire stay in

Buffalo.

In the end, but most importantly, I would like to thank my parents, who have always supported my dreams

and motivated me to achieve them. I dedicate this work to their unending love and support, even when I was

distant from home.

Thank you all.

v | P a g e

Table of Contents

Acknowledgement ................................................................................................................................... iv

Table of Contents ..................................................................................................................................... v

Table of Figures ....................................................................................................................................... ix

Table of Equations................................................................................................................................... xi

Abstract ..................................................................................................................................................... 1

1. Introduction ....................................................................................................................................... 3

1.1. Case Study: Hexapod Parallel Platform Manipulator .................................................................... 4

1.2. Goals of this work ......................................................................................................................... 5

1.3. Model Simplification ..................................................................................................................... 6

1.4. Organization of the rest of the Thesis ........................................................................................... 8

2. Literature Review .............................................................................................................................. 9

2.1. Related work ................................................................................................................................ 9

2.1.1. Modeling and Analysis of parallel manipulators ..................................................................... 9

2.1.2. Workspace Analysis of parallel manipulators ....................................................................... 10

3. Mathematical and Technological Tools ......................................................................................... 12

3.1. Kinematic Analysis ..................................................................................................................... 12

3.1.1. Framework .......................................................................................................................... 12

3.1.2. Example: 3-RPR.................................................................................................................. 13

3.1.2.1. Nomenclature ................................................................................................................. 13

3.1.2.2. Case 1: ‘cut’ at Point D: all joint variables ....................................................................... 14

3.1.2.3. Case 2: ‘cut’ at Point D: all joint Variables and Task Variables ....................................... 15

3.1.2.4. Case 3: ‘cut’ at Point C: 6 joint Variables and Task Variables ......................................... 16

3.2. Dynamic Modeling (Lagrangian method) .................................................................................... 17

vi | P a g e

3.3. Gauss Divergence Theorem ....................................................................................................... 19

3.3.1. Theorem Statement ............................................................................................................. 19

3.3.2. Application to Workspace Volume ....................................................................................... 19

3.3.2.1. Example - 3-RPR mechanism ......................................................................................... 20

3.4. Simulink/Simmechanics .............................................................................................................. 21

3.5. MapleSim ................................................................................................................................... 21

4. Workspace Analysis ........................................................................................................................ 23

4.1. 3PRR ......................................................................................................................................... 24

4.1.1. Nomenclature ...................................................................................................................... 24

4.1.2. Parametric Sweeps with Inverse Position Kinematics .......................................................... 25

4.1.3. Recursive nearest neighbor search with Inverse Position Kinematics .................................. 26

4.1.4. Exact workspace determination using geometric constraints ............................................... 27

4.2. Hexapod ..................................................................................................................................... 31

4.2.1. Nomenclature ...................................................................................................................... 31

4.2.2. Parametric Sweeps with Inverse Position Kinematics .......................................................... 33

4.2.3. Parametric Sweeps with Geometric checks ......................................................................... 34

4.2.4. Recursive nearest neighbor search using Inverse Position Kinematics ............................... 36

4.2.5. Recursive nearest neighbor search using Geometric checks .............................................. 36

4.2.6. Exact Workspace determination using Geometric constraints ............................................. 38

4.2.7. Using CAD modeling software to compute volumes ............................................................ 40

4.3. Stewart Platform ......................................................................................................................... 43

5. Kinematic and Dynamic Analysis of 3-PRR ................................................................................... 44


vii | P a g e

5.2. Dynamic Analysis ....................................................................................................................... 47

5.2.1. Hand derivation ................................................................................................................... 47

5.2.2. Maple derivation .................................................................................................................. 48

5.2.3. MapleSim Modeling ............................................................................................................. 49

5.3. Matlab Kinematic Simulation ...................................................................................................... 51

5.3.1. Calculation of Initial Pose .................................................................................................... 51

5.3.2. Final Simulation Run ........................................................................................................... 51

5.4. Simmechanics Forward Dynamics Simulation Comparison ........................................................ 52

5.5. Simmechanics Control Dynamics Simulation .............................................................................. 54

6. Kinematic and Dynamic Analysis and Control of Hexapod .......................................................... 57


6.2. Dynamic Analysis ....................................................................................................................... 59

6.3. Forward Dynamics comparison between Matlab/Simmechanics/Maplesim ................................ 59

6.4. Control Dynamics Simulation with Simmechanics ...................................................................... 62

6.5. Further steps for Real-time implementation ................................................................................ 62

7. MapleSim for Education .................................................................................................................. 63

8. Future Scope ................................................................................................................................... 64

9. Conclusion ....................................................................................................................................... 64

10. References ................................................................................................................................... 64

11. Appendix ................................................................................................................................... 11-1

11.1. Parts and Specification ........................................................................................................ 11-1

11.2. 3-RPR Kinematics ............................................................................................................... 11-1

11.2.1. 9 joint variables set .......................................................................................................... 11-1

11.2.1.1. Positions and velocities of all points ........................................................................... 11-1

viii | P a g e

11.2.2. Set of 6 joint variables and 3 task variables ..................................................................... 11-2

11.3. Gauss Divergence Theorem Calculations ............................................................................ 11-3

11.3.1. Circular Arc ...................................................................................................................... 11-3

11.3.2. Line ................................................................................................................................. 11-3

11.4. Maple Code for Hexapod Dynamic Analysis ........................................................................ 11-4

11.5. 3PRR Dynamic Equations.................................................................................................... 11-7

11.5.1. Substitutions .................................................................................................................... 11-7

11.5.2. 7 variable set ................................................................................................................... 11-8

11.5.2.1. Force Vector .............................................................................................................. 11-8

11.5.2.2. Mass Matrix ............................................................................................................... 11-8

11.5.2.3. Constraint Matrix ........................................................................................................ 11-8

11.6. Hexapod Dynamic Equations ............................................................................................... 11-9

11.6.1. Substitutions .................................................................................................................... 11-9

11.6.2. 21 variable set ................................................................................................................. 11-9

11.6.2.1. Force Vector .............................................................................................................. 11-9

11.6.2.2. Mass Matrix ............................................................................................................. 11-10

11.6.2.3. Constraint Matrix ...................................................................................................... 11-11

11.6.2.4. Torque Dissipation Matrix ........................................................................................ 11-12

11.6.3. 24 variable set ............................................................................................................... 11-12

11.6.3.1. Force Vector ............................................................................................................ 11-12

11.6.3.2. Mass Matrix ............................................................................................................. 11-12


11.6.3.4. Torque Dissipation Matrix ........................................................................................ 11-13

ix | P a g e

11.6.4. 6 joint variables + 6 task variables set ........................................................................... 11-14


Table of Figures

Figure 1 - Hexapod ..................................................................................................................................... 4

Figure 2 - 3PRR and 3RPS Models ............................................................................................................ 6

Figure 3 - Hexapod model Simplification to 3-PRR ..................................................................................... 8

Figure 4 : 3RPR nomenclature ................................................................................................................. 13

Figure 5 - Constant Orientation Workspace of 3RPR (φ =0) ..................................................................... 20

Figure 6 - Constant Orientation Workspace of 3RPR (phi=0) using brute force method (dark black

patch) and Gauss divergence method (thick red solid line) .................................................... 21

Figure 7 - (a) Simple pendulum problem; (b) modeled in MapleSim environment; (c) generated EOMs,

plot of results from forward dynamic simulation, and visualization of the pendulum in

motion. .................................................................................................................................. 23

Figure 8: Simplified 2-D 3PRR system ..................................................................................................... 24

Figure 9 - Workspace of 3PRR with grid size 0.01 .................................................................................... 26

Figure 10 : Recursive nearest neighbor search algorithm ......................................................................... 27

Figure 11 - Individual PRR workspaces in 3PRR ...................................................................................... 28

Figure 12 - Shifted individual PRR workspaces for 3PRR ......................................................................... 29

Figure 13 - Overlay of 3PRR individual workspaces with brute-force method workspace ......................... 29

Figure 14 - Workspace calculation using Gauss divergence - intersection Points ..................................... 30

Figure 15 - 3PRR constant orientation workspace for all orientations ....................................................... 31

Figure 16 - Hexapod Structure and Reference frames ............................................................................. 32

Figure 17 : Hexapod TLS ......................................................................................................................... 32

Figure 18 - Hexapod workspace for [0 0 0] orientation using brute force method with inverse position

kinematics routine .................................................................................................................. 33

x | P a g e

Figure 19 – Vector diagram for Geometry-based checks on Hexapod ...................................................... 34

Figure 20 – Geometric check method - rmin ............................................................................................... 35

Figure 21 - Geometric check method - rmax ............................................................................................... 35

Figure 22 - Hexapod workspace for [0 0 0] orientation using brute force method with geometric

checks ................................................................................................................................... 36

Figure 23 : Comparative study of all grid search methods (a) log-log graph of implementation time

w.r.t. grid size (b) log-log graph of % Volume error w.r.t. grid size ......................................... 37

Figure 24 : Vector loop for Exact workspace using geometric constraints ................................................ 38

Figure 25 – Exact Workspace using Geometric checks - 3D Workspace .................................................. 39

Figure 26 : Comparison of workspace from grid-search method (Shaded) and Exact workspace

method (Solid line) ................................................................................................................. 40

Figure 27 - 3D [0 0 0] orientation workspace by Exact Workspace method .............................................. 41

Figure 28 - Solidworks representation of constant orientation workspace of Hexapod at [0 0 0]

orientation .............................................................................................................................. 41

Figure 29 : Hexapod at Orientation [30 20 0] degrees .............................................................................. 41

Figure 30 : [30 20 0] orientation workspace using Exact workspace method (MATLAB) ........................... 42

Figure 31 : 3D Workspace by Exact workspace CAD method- ([α, β, γ]T = [30, 20, 0]) ............................. 42

Figure 32 : Workspace volume Optimization results from CAD model ...................................................... 43

Figure 33 - Comparison of Stewart Gough platform results with Yoshito paper [15] ................................. 43

Figure 34 : Maplesim model for Hexapod ................................................................................................. 50

Figure 35 - Initial Pose of 3PRR for dynamic Simulation ........................................................................... 52

Figure 36 - 3PRR Forward dynamics setup - Simmechanics .................................................................... 52

Figure 37 - 3PRR forward dynamics model - Simmechanics .................................................................... 53

Figure 38 - Maplesim/Matlab Forward dynamics comparison ................................................................... 54

Figure 39 - 3PRR Control dynamics setup - Simmechanics ..................................................................... 55

Figure 40 - Maplesim/Matlab control dynamics simulation ........................................................................ 56

Figure 41 : Kinematic Validation of 12 variable formulation of Hexapod ................................................... 58

xi | P a g e

Figure 42 - Hexapod Maplesim model ...................................................................................................... 59

Figure 43 : Overall model of Hexapod in Simmechanics ........................................................................... 60

Figure 44 : Forward dynamics model of Hexapod in Simmechanics ......................................................... 60

Figure 45 : Maplesim/Matlab forward dynamics comparison (a)Difference in prismatic positions

calculated (b) Difference in prismatic velocities calculated (c) Actual prismatic positions

calculated in Matlab (d) Position constraints (convergent) ..................................................... 61

Table of Equations

Equation 3-1 : Position constraint equation format .................................................................................... 12

Equation 3-2 : Velocity constraint equation format .................................................................................... 12

Equation 3-3 : Velocity constraints in terms of dependent and independent variables .............................. 12

Equation 3-4 : Velocity relation between dependent and independent variables ....................................... 12

Equation 3-5 : Acceleration constraints between dependent and independent variables .......................... 13

Equation 3-6 : 3RPR central platform position/orientation ......................................................................... 14

Equation 3-7 : 3RPR central platform velocities ........................................................................................ 14

Equation 3-8 : 3RPR Jacobian representation .......................................................................................... 14

Equation 3-9 : 3RPR, Case 1, variable sets .............................................................................................. 14

Equation 3-10 : 3RPR, Case 1, position constraints ................................................................................. 14

Equation 3-11 : 3RPR, Case 1, velocity constraints .................................................................................. 15

Equation 3-12 : 3RPR, Case 2, Variable sets ........................................................................................... 15

Equation 3-13 : 3RPR, Case 2, Position constraints ................................................................................. 15

Equation 3-14 : 3RPR, Case 2, Velocity constraints ................................................................................. 16

Equation 3-15 : 3RPR, Case 2, Variable sets ........................................................................................... 16

Equation 3-16 - Lagrangian formulation equation ..................................................................................... 17

Equation 3-17 : General dynamic equation for constrained systems ........................................................ 17

Equation 3-18 : Dynamic terms related to EOM derivation for 3RPR ........................................................ 18

Equation 3-19 : Euler Lagrange formulation for part of EOM of 3RPR ...................................................... 18

Equation 3-20 : Mathematical statement of Gauss divergence theorem ................................................... 19

xii | P a g e

Equation 3-21 : Mathematical statement for Gauss divergence Theorem (2D) ......................................... 19

Equation 3-22 : Gauss divergence theorem adaptation for area measurement (LHS) .............................. 19

Equation 4-1 : Basic Vector loop closure Equation for Hexapod ............................................................... 34

Equation 4-2 : rmax and rmin values for geometric checks method .............................................................. 35

Equation 4-3 : Vector loop closure Equation for Geometry constraints modeling of Hexapod ................... 38

Equation 4-4 : Rearranged vector loop closure equation for Geometry constraints modeling of

Hexapod ................................................................................................................................ 38

Equation 4-5 : Possible combinations of three sphere intersection points for Hexapod constant

orientation workspace ............................................................................................................ 39

Equation 5-1 : 3PRR – Position of link masses ......................................................................................... 44

Equation 5-2 : 3PRR - Position of Center of Platform ............................................................................... 44

Equation 5-3 : 3PRR – Velocities of all points in mechanism .................................................................... 45

Equation 5-4 : 3PRR – angle relationships ............................................................................................... 45

Equation 5-5 : 3PRR – Jacobian representation ....................................................................................... 45

Equation 5-6 - Overall kinematic Constraints ............................................................................................ 46

Equation 5-7 - DOF wise Constraints ....................................................................................................... 46

Equation 5-8 - Independent DOF calculation ............................................................................................ 46

Equation 5-9 - Null Space Component Calculation ................................................................................... 46

Equation 5-10 : Hand derivation of 3PRR dynamic EOMs ........................................................................ 47

Equation 5-11 : Dynamic matrices for the 3PRR (9 joint variables set) ..................................................... 48

Equation 5-12 - Reference frames and parameters .................................................................................. 49

Equation 13 : Forward dynamic simulation equation ................................................................................. 53

Equation 14 : Force projection method for Inverse dynamics ................................................................... 55

Equation 15 : Inverse dynamic simulation equation .................................................................................. 55

Equation 16 : Inverse dynamic simulation overall equation ....................................................................... 55

Equation 11-1 : Area under circular arc using Gauss divergence theorem ............................................ 11-3

Equation 11-2 : Area under line using Gauss divergence theorem ........................................................ 11-3

1 | P a g e

Abstract

A parallel manipulator is a closed loop kinematic chain mechanism that is connected to the base via

multiple independent kinematic chains. Parallel manipulators have found numerous applications in

industries ranging from airplane simulators to high-speed pick and place robots due to their higher load-

carrying capacities and improved stiffness as compared to their serial counterparts. These advantages

arise due to the presence of kinematic-closed loops within the manipulator that allow for the load to be

transmitted to the ground via multiple chains. However, these kinematic closed loops also cause the

workspace of parallel manipulators to be severely limited and also pose a major challenge to their analysis

and control. Another problem is the evaluation of the workspace that is simultaneous affected by so many

geometric parameters of the parallel manipulator.

Parallel platform manipulators consist of a central platform attached to the base via multiple legs. One

of the most popular parallel manipulators is the 6-DOF Stewart-Gough platform. It has 6-UPS architecture

and has been the focus of much work till date. However, the main problem is the power consumption of the

manipulator because of the bulky prismatic actuators lying along the links. Hence, we explore a novel

design of a parallel manipulator with 6PUS architecture that has the prismatic actuators attached to the

base, thereby making the legs lighter and the device energy efficient on the whole. We will refer to this

novel design as the “Hexapod” for the rest of this work. The goal of this work is to look into the following

aspects of parallel manipulators with special reference to the Hexapod:

• Workspace analysis

• Modeling, Equation of Motion (EOM) generation and simulation

In this work, we dwell into the workspace analysis of parallel platform manipulators. Since the

workspace of a parallel manipulator is quite complex, it takes a considerable amount of time to compute it.

However, in order to optimize a parallel mechanism for workspace, an efficient and universal way of

workspace analysis is necessary. We propose an improved method of calculating the constant orientation

workspace of any parallel platform manipulator. Finally, we use a CAD package to speed up the process to

make a case for CAD-enhanced workspace analysis and showcase its general application to any parallel

platform manipulator.

2 | P a g e

Another study explores a novel method for computing the equations of motion automatically by using a

symbolic computation engine called Maple. We also explore how this method of symbolic equation

generation, coupled with an easy to use interface called Maplesim; can be beneficial in augmenting the

learning in robotics courses. The next task is to employ these EOM’s to formulate control strategies and

test them in simulations.

3 | P a g e

1. Introduction

A parallel platform manipulator (PPM) is a device whose end-effector is attached to the ground via

multiple serial chains that provides closed kinematic loops in the system for better load handling capacity

and stiffness. It is these qualities that make it applicable in a wide range of applications ranging from flight

simulators to high speed milling machines. However, it is these closed kinematic loops that increase the

complexity of their analysis to a great extent.

An important advantage of parallel platform manipulator is that its superior structural rigidity renders it

as the better choice over serial chain manipulator (SCM) for moving heavy loads and high precision

machining tasks. The advantages include very high accuracy, better stiffness ratio, more payload capacity

and better inertial distribution among others.

Owing to their rolling and tilting capabilities and rigidity imparted by the closed loop coupled

architecture, the parallel platform robots serve well as moving platforms that perform tasks requiring

flexibility, precision and rigidity. Some of their end applications include tool base for multi-axis CNC

machining, flight simulator, simulation of wave induced motion on naval cargo ships, telescopes, fine

positioning devices and fast packaging among others.

Despite the advantages that PPMs bring about in articulated mechanical activities, a down side

remains: the supporting links that form coupled closed loop kinematic chains add further constraints into the

system resulting in restricted work space as compared to SCMs.

Also, the high non-linearity generated in the loop closure equation for the Hexapod (Hex/H) platform

adds to the complexity of the problem as the link interference constraints cannot be fully detected

analytically. This situation creates a demand for an efficient numerical computational approach that also

incorporates the analytical techniques. Parallel robots are highly sensitive to dimensioning; hence accuracy

is a key issue in such applications.

Finally, this thesis is intended as a learning ground for understanding large and complex articulated

multi-body systems (AMBS). The Hexapod, with its multiple parts coupled together by multiple joints that

can be categorized as 6 screw joints (SJ), 6 universal joints (UJ) and 3 spherical joints (SJ), is a complex

example illustrating the mechanics and control in a complex multi-body system. Also, the complexity of this

4 | P a g e

system demands the use of different analytical techniques, numerical simulation tools, software

environments and computer-hardware interfaces, thus providing enough opportunities for practicing and

honing the skills learnt through the coursework.

1.1. Case Study: Hexapod Parallel Platform Manipulator

In this thesis work, the focus is placed on the study of a Novel 6 degree-of-freedom (DOF) PPM

referred to as a Hexapod.

Figure 1 - Hexapod

This device is capable of moving high payload at high acceleration within a compact workspace. The

platform is joined to rigid fixed length arms at three points that form an equilateral triangle. The arms are

linked to the platform in pairs, each pair coming together at one of their ends and subsequently attached to

the manipulator though a spherical joint. The free ends of each of the six arms are linked to the base by a

universal joint which can slide on the base. The bases of the arms are along prismatic joints. The same pair

of links that join together at the platform occupy one of the straight line axes, thus there are three straight

horizontal slots on the platform, each with two link bases sliding in it. The three slots are mutually aligned

on the rigid base such that they are collinear to each of three arms of an equilateral triangle.

5 | P a g e

1.2. Goals of this work

The goals of this work relate to the following aspects of parallel manipulators:

• Workspace Analysis: Workspaces of parallel manipulators are quite complex in shape and hence,

not easily computed. Numerical techniques are generally slow for accurate results and inaccurate

when fast results are desired. We only focus on the constant orientation workspace of parallel

manipulators. We change the normal approach of an exhaustive grid search using inverse position

kinematics routine in three ways:

• Improve each computation by replacing inverse position kinematics with a geometric check

routine

• Decrease number of computations by replacing exhaustive search with recursive nearest

neighbor search algorithm

• Eliminate grid search altogether by using a geometric constraint algorithm that finds the

exact curves bounding the workspace

• EOM generation for kinematic and dynamic analysis, simulation and model-based control : We

explore how the hand-derivation of complex EOMs for parallel manipulators can be simplified by

using a symbolic computation engine, Maple. We also examine an automatic block-based modeling

tool, Maplesim, built onto Maple and how this tool can not only further help alleviate many

drawbacks associated with hand-derivation but also eliminate the need for programmed symbolic

computation. Finally, we validate the EOM and forward dynamics simulation results against

Simmechanics and attempt to develop control simulations for the Hexapod.

• In an additional study, we also delve into the usefulness of Maplesim for augmenting learning in

basic robotics courses.

6 | P a g e

1.3. Model Simplification

Due to the 3D structure of the model and numerous joints, a dynamic analysis and control was difficult

from scratch. Hence, the model was simplified by taking a 2D projection from the top and side views.

Figure 2 - 3PRR and 3RPS Models

In the top view, each pair of moving slider blocks (prismatic joints) was simplified to single blocks

sliding along the rails. The universal and prismatic joints were assumed as revolute joints. Thus this

simplification converts the three dimensional system to a two dimensional 3-PRR (Prismatic-Revolute-

7 | P a g e

Revolute) manipulator. The resulting model is as shown. This system emulates three of the DOF of the

Hexapod, i.e. 2 displacements (x and y axes) and one rotation (about z axis).

In the side view, each pair of sliders and universal joints simplified to a revolute-prismatic joint. Hence,

a 3RPS (Revolute Prismatic Spherical) system resulted. This system emulates the remaining three DOF of

the Hexapod, i.e. one translation (z axis) and two rotations (about x and y axes).

At the end, we model the Hexapod and try to control it using kinematic and dynamic models.

Next, we restrict three of its DOF so as to simplify it to resemble a 3-DOF 3-PRR.

We set the angle of the platform to [0,0,φ] and the position of the center of the platform to be [x,y,z0],

where z0 is a constant. The Hexapod thus simplifies as shown:

-2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5

-2

-1

0

1

2

3

0

1

2

X axis

Y axis

Z ax

is

8 | P a g e

Figure 3 - Hexapod model Simplification to 3-PRR

From the above diagram, it is inferred that the variations in s1 and s2 have a one-to-one mapping with

the xlocal and ylocal values with fixed z=z0 and ay=az=0; which in turn are directly dependent on the [x, y, φ] of

the central platform (for 3PRR). Hence, we can say that in this scenario, the 3PRR is a quotient

manipulator of the Hexapod.

1.4. Organization of the rest of the Thesis

In Section 2, we survey some of the work done on kinematic and dynamic modeling and control as well

as workspace analysis of PPMs. In Section 3, we explore all the mathematical and technological tools used

in this thesis. In section 4, we undertake the workspace analysis of the 3PRR and Hexapod and showcase

methods to make the process more efficient. In Sections 5 and 6, we see how the concepts of kinematic

and dynamic modeling and control are applied to a 3PRR and consecutively the Hexapod. In Section 7, we

conduct a review of the effort made to develop MapleSim, one of the technological tools, for augmenting

learning in basic robotics courses. Finally, we discuss the future scope and conclude.

9 | P a g e

2. Literature Review

2.1. Related work

2.1.1. Modeling and Analysis of parallel manipulators

Many efforts have been made in the past to model the kinematics and dynamics of parallel

manipulators in order to build model based controllers that have been found to be much more stable than

their traditional counterparts.

Merlet [1], also presents an excellent treatise on parallel robots in his book on Parallel Robots. It

gradually introduces the reader to parallel robot through its historical development, basic architecture,

kinematic analysis, static and dynamic analysis, determination of singular configurations and workspace

and finally design.

A rather detailed and generic mathematical treatment of Parallel Manipulators has been discussed by

Merlet [2]. This paper presents a general mathematical framework to estimate the Jacobian, or more

precisely the inverse Jacobian matrices. It focuses on end-effector motions that cannot be detected by

measurement of the actuated joint space parameters.

One of the most common parallel platform manipulators, the Stewart-Gough platform, has been the

topic of much research [3-5] till date.

Xi and Sinatra [6], presented an analytical study of the inverse dynamics of hexapods with fixed-length

legs. The hexapod model used is a chain Prismatic-Universal-Spherical links connecting the base to the

platform through six legs. The inverse kinematic analysis including the Jacobian calculation has been done

using the loop-closure equations. The inverse dynamic calculations have been done through Newton-Euler

formulations and natural orthogonal complements.

Madu Sathianarayan [7], attempted a vector analysis of the 6-PUS manipulator in his thesis and

showed that it is more energy efficient than the 6-UPS architecture of the Stewart Gough platform.

Another active area of research has been the simplification of a complex manipulator into smaller,

simpler quotient manipulators. Meng et. al. [8], explored the concept to set up a geometric theory for

analysis and synthesis of serial manipulator sub-chains given the desired motion type of a sub-6DOF

parallel manipulator.

10 | P a g e

Many efforts have been made to compute the kinematic and dynamic models or planar parallel

manipulators like 3PRR, 3RPR etc. Staicu [9], calculated the inverse dynamics of the 3PRR mechanism

using the principle of virtual work. Staicu [10], also solved the inverse dynamics of the 3RPR mechanism

using the principle of virtual work followed by a power requirement analysis of two actuation schemes –

prismatic actuators and revolute actuators.

2.1.2. Workspace Analysis of parallel manipulators

Several attempts in the past have succeeded in studying the workspace of 6-DOF parallel platform

manipulators. Some early attempts like Saxena et. al. [11] were able to describe the constant orientation

workspace using the parameter sweep method. Bonev and Gosselin in [12] proposed the geometric

approach to handling workspaces of parallel manipulators using a 6-R-U-S mechanism as a case study.

Thereafter, there has been considerable research concerning the constant orientation workspace of 6-DOF

PPMs but very few of them concentrated on symbolically solving and automating this procedure. An

attempt was made [13] to find the total and singularity free orientation workspaces (previously referred to as

constant position workspace) of the Stewart platform. Merlet et. al. [14], was able to find various subsets of

the orientation workspace. The constant position workspace also can be solved by using geometric

methods but the analysis was incomplete at the time the thesis was written.

Yoshito et. al. [15], attempted to use the intersection of multiple spheres describing the feasible

movements of all the different serial manipulators. However, the work remained limited to the visualization

case and no efforts at quantization were pursued i.e. the intersection volumes were never computed nor

were any optimization methods deployed. A similar intersection-workspace visualization concept was used

[16] a spatial 6-PRRS using CATIA programmatic modeling approach. In contrast, in this paper, we seek to

use the CAD package to both (i) characterize various aspects of the workspace using quantitative-

measures; as well as (ii) use workspace performance-measures for optimization to achieve another level of

automation in a mechanism design process.

In a more recent study by Ngoc et. al. [17], a 6-DOF parallel platform manipulator of Prismatic-

Spherical-Universal (PSU) sequence of joints has been simulated on a prototype simulator. Inverse position

analysis, Jacobian analysis and workspace analysis have been done before a successful single operator

11 | P a g e

ride. This prototype developed is somewhat similar to the Hexapod which is a Prismatic-Universal-Spherical

(PUS) 6-DOF-PM.

12 | P a g e

3. Mathematical and Technological Tools

3.1. Kinematic Analysis

The main difference between analysis of parallel and serial manipulators is the constraints relating

different states of the system. These constraints allow only some of the states of the system to be

independently controlled while the rest become dependent. The choice of dependent and independent

states can depend on the choice of formulation.

For a general parallel manipulator with n states and m degrees of freedom (or independent states),

there would be n-m constraints.

So, the vector of n states, q, can be broken up into a vector with (n-m) dependent states, qdep and

another vector with m independent states, qind.

There are many different formulations available for the same system, each depending upon the choice

of dependent and independent variables, which actually depends upon where the mechanism is ‘cut’ to

create the constraints. To explore these choices, a 3-RPR mechanism is considered as an example.

3.1.1. Framework

The Position constraints in the system, generally represented by C, are written as

𝐶𝐶 = 0

Equation 3-1 : Position constraint equation format

On differentiating the position constraints, the velocity constraints in the system are obtained and can

be written as

𝐴𝐴 ∙ ��𝑞 = 0

Equation 3-2 : Velocity constraint equation format

This equation can be decoupled into the independent and dependent states and can be rewritten as

𝐴𝐴𝑑𝑑𝑑𝑑𝑑𝑑 q𝑑𝑑𝑑𝑑𝑑𝑑 + 𝐴𝐴𝑖𝑖𝑖𝑖𝑑𝑑 q𝑖𝑖𝑖𝑖𝑑𝑑 = 0

Equation 3-3 : Velocity constraints in terms of dependent and independent variables

Hence, the dependent states can be expressed as a function of the independent states as

∴ q𝑑𝑑𝑑𝑑𝑑𝑑 = 𝐷𝐷 ∙ ��𝑞𝑖𝑖𝑖𝑖𝑑𝑑 , 𝐷𝐷 = −𝐴𝐴𝑑𝑑𝑑𝑑𝑑𝑑 −1 ∙ 𝐴𝐴𝑖𝑖𝑖𝑖𝑑𝑑

Equation 3-4 : Velocity relation between dependent and independent variables

13 | P a g e

On differentiating Equation 3-3, we get the acceleration level constraints

��𝐴 ∙ ��𝑞 + 𝐴𝐴 ∙ ��𝑞 = 0 → ��𝐴 ∙ ��𝑞 + 𝐴𝐴𝑑𝑑𝑑𝑑𝑑𝑑 ∙ ��𝑞𝑑𝑑𝑑𝑑𝑑𝑑 + 𝐴𝐴𝑖𝑖𝑖𝑖𝑑𝑑 ∙ ��𝑞𝑖𝑖𝑖𝑖𝑑𝑑 = 0

Equation 3-5 : Acceleration constraints between dependent and independent variables

Hence, the ��𝑞𝑑𝑑𝑑𝑑𝑑𝑑 can again be expressed in terms of ��𝑞𝑖𝑖𝑖𝑖𝑑𝑑 and ��𝑞.

3.1.2. Example: 3-RPR

3.1.2.1. Nomenclature

Figure 4 : 3RPR nomenclature

Bi : base revolute joint position for ith RPR subsystem

bxi : x-coordinate of base for ith RPR subsystem

byi : y-coordinate of base for ith RPR subsystem

Ci : second revolute joint connecting to the platform for ith RPR subsystem

D : end point of the center

si : prismatic joint length for ith RPR subsystem

li1 : length of first link for ith RPR subsystem

li2 : length of platform link for ith RPR subsystem

Hence, location and orientation of point D is expressed as

𝑋𝑋𝑑𝑑𝑖𝑖 = 𝑏𝑏𝑏𝑏𝑖𝑖 + (𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖) ∗ cos(𝜃𝜃𝑖𝑖1) + 𝑙𝑙𝑖𝑖2 ∗ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)

𝑌𝑌𝑑𝑑𝑖𝑖 = 𝑏𝑏𝑏𝑏𝑖𝑖 + (𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖) ∗ sin(𝜃𝜃𝑖𝑖1) + 𝑙𝑙𝑖𝑖2 ∗ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)

14 | P a g e

𝜃𝜃𝑖𝑖 = 𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2

Equation 3-6 : 3RPR central platform position/orientation

Differentiating,

𝑋𝑋��𝑑𝑖𝑖 = {−(𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖) ∗ sin(𝜃𝜃𝑖𝑖1) − 𝑙𝑙𝑖𝑖2 ∗ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)} ∗ ��𝜃𝑖𝑖1 + ��𝑠𝑖𝑖 ∗ 𝑐𝑐𝑐𝑐𝑠𝑠(𝜃𝜃𝑖𝑖1)− 𝑙𝑙𝑖𝑖2 ∗ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2) ∗ ��𝜃𝑖𝑖2

𝑌𝑌��𝑑𝑖𝑖 = {(𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖) ∗ cos(𝜃𝜃𝑖𝑖1) + 𝑙𝑙𝑖𝑖2 ∗ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)} ∗ ��𝜃𝑖𝑖1 + ��𝑠𝑖𝑖 ∗ 𝑠𝑠𝑖𝑖𝑖𝑖(𝜃𝜃𝑖𝑖1) + 𝑙𝑙𝑖𝑖2 ∗ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2) ∗ ��𝜃𝑖𝑖2

��𝜃𝑖𝑖 = ��𝜃𝑖𝑖1 + ��𝜃𝑖𝑖2

Equation 3-7 : 3RPR central platform velocities

This can be rewritten as,

�𝑋𝑋��𝑑𝑖𝑖𝑌𝑌��𝑑𝑖𝑖��𝜃𝑖𝑖� = �

−(𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖) ∗ sin(𝜃𝜃𝑖𝑖1)− 𝑙𝑙𝑖𝑖2 ∗ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2) cos(𝜃𝜃𝑖𝑖1) −𝑙𝑙𝑖𝑖2 ∗ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)(𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖) ∗ cos(𝜃𝜃𝑖𝑖1) + 𝑙𝑙𝑖𝑖2 ∗ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2) 𝑠𝑠𝑖𝑖𝑖𝑖(𝜃𝜃𝑖𝑖1) 𝑙𝑙𝑖𝑖2 ∗ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)

1 0 1� ∙ �

��𝜃𝑖𝑖1��𝑠𝑖𝑖��𝜃𝑖𝑖2

�

𝑋𝑋��𝑋𝑖𝑖 = 𝐽𝐽𝑖𝑖 ∙ ��𝑞𝑖𝑖

Equation 3-8 : 3RPR Jacobian representation

3.1.2.2. Case 1: ‘cut’ at Point D: all joint variables

States, n=9

DOF, m=3 (independent states)

Constraints, n-m=6 (dependent states)

𝑞𝑞 = [𝜃𝜃11, 𝑠𝑠1,𝜃𝜃12,𝜃𝜃21, 𝑠𝑠2,𝜃𝜃22,𝜃𝜃31, 𝑠𝑠3,𝜃𝜃32]

��𝑞 = [��𝜃11, ��𝑠1, ��𝜃12, ��𝜃21, ��𝑠2, ��𝜃22, ��𝜃31, ��𝑠3, ��𝜃32]

��𝑞𝑖𝑖𝑖𝑖𝑑𝑑 = ��𝜃11, ��𝑠1, ��𝜃12�𝑎𝑎𝑖𝑖𝑑𝑑 ��𝑞𝑑𝑑𝑑𝑑𝑑𝑑 = [��𝜃21, ��𝑠2, ��𝜃22, ��𝜃31, ��𝑠3, ��𝜃32]

Equation 3-9 : 3RPR, Case 1, variable sets

𝐶𝐶 =

⎣⎢⎢⎢⎢⎡𝑋𝑋𝑑𝑑1 − 𝑋𝑋𝑑𝑑2𝑌𝑌𝑑𝑑1 − 𝑌𝑌𝑑𝑑2𝜃𝜃1 − 𝜃𝜃2𝑋𝑋𝑑𝑑1 − 𝑋𝑋𝑑𝑑3𝑌𝑌𝑑𝑑1 − 𝑌𝑌𝑑𝑑3𝜃𝜃1 − 𝜃𝜃3 ⎦

⎥⎥⎥⎥⎤

𝐶𝐶 =

⎣⎢⎢⎢⎢⎡𝑏𝑏𝑏𝑏1 + (𝑙𝑙11 + 𝑠𝑠1) ∗ cos(𝜃𝜃11) + 𝑙𝑙12 ∗ cos(𝜃𝜃11 + 𝜃𝜃12)− 𝑏𝑏𝑏𝑏2 − (𝑙𝑙21 + 𝑠𝑠2) ∗ cos(𝜃𝜃21) − 𝑙𝑙22 ∗ cos(𝜃𝜃21 + 𝜃𝜃22)𝑏𝑏𝑏𝑏1 + (𝑙𝑙11 + 𝑠𝑠1) ∗ sin(𝜃𝜃11) + 𝑙𝑙12 ∗ sin(𝜃𝜃11 + 𝜃𝜃12)− 𝑏𝑏𝑏𝑏2 − (𝑙𝑙21 + 𝑠𝑠2) ∗ sin(𝜃𝜃21) − 𝑙𝑙22 ∗ sin(𝜃𝜃21 + 𝜃𝜃22)

𝜃𝜃11 + 𝜃𝜃12 − 𝜃𝜃21 − 𝜃𝜃22𝑏𝑏𝑏𝑏1 + (𝑙𝑙11 + 𝑠𝑠1) ∗ cos(𝜃𝜃11) + 𝑙𝑙12 ∗ cos(𝜃𝜃11 + 𝜃𝜃12)− 𝑏𝑏𝑏𝑏3 − (𝑙𝑙31 + 𝑠𝑠3) ∗ cos(𝜃𝜃31) − 𝑙𝑙32 ∗ cos(𝜃𝜃31 + 𝜃𝜃32)𝑏𝑏𝑏𝑏1 + (𝑙𝑙11 + 𝑠𝑠1) ∗ sin(𝜃𝜃11) + 𝑙𝑙12 ∗ sin(𝜃𝜃11 + 𝜃𝜃12)− 𝑏𝑏𝑏𝑏3 − (𝑙𝑙31 + 𝑠𝑠3) ∗ sin(𝜃𝜃31) − 𝑙𝑙32 ∗ sin(𝜃𝜃31 + 𝜃𝜃32)

𝜃𝜃11 + 𝜃𝜃12 − 𝜃𝜃31 − 𝜃𝜃32 ⎦⎥⎥⎥⎥⎤

Equation 3-10 : 3RPR, Case 1, position constraints

15 | P a g e

Hence, differentiating the position constraint matrix C results in

��𝐶 =

⎣⎢⎢⎢⎢⎢⎡𝑋𝑋��𝑑1 − 𝑋𝑋��𝑑2𝑌𝑌��𝑑1 − 𝑌𝑌��𝑑2��𝜃1 − ��𝜃2𝑋𝑋��𝑑1 − 𝑋𝑋𝑑𝑑3𝑌𝑌��𝑑1 − 𝑌𝑌��𝑑3��𝜃1 − ��𝜃3 ⎦

⎥⎥⎥⎥⎥⎤

= �𝐽𝐽1 ∙ ��𝑞1 − 𝐽𝐽2 ∙ ��𝑞2𝐽𝐽1 ∙ ��𝑞1 − 𝐽𝐽3 ∙ ��𝑞3

� = �𝐽𝐽1 −𝐽𝐽2 0𝐽𝐽1 0 −𝐽𝐽3

� ∙ ��𝑞

q𝑑𝑑𝑑𝑑𝑑𝑑 = q(4: 9), q𝑖𝑖𝑖𝑖𝑑𝑑 = q(1: 3)

𝐴𝐴𝑑𝑑𝑑𝑑𝑑𝑑 = 𝐴𝐴(: ,4: 9),𝐴𝐴𝑖𝑖𝑖𝑖𝑑𝑑 = 𝐴𝐴(: ,1: 3)

Equation 3-11 : 3RPR, Case 1, velocity constraints

3.1.2.3. Case 2: ‘cut’ at Point D: all joint Variables and Task Variables

In this case, the only difference is that the task space variables are selected as independent and all

other variables are set as dependent.

States, n=12



𝑞𝑞 = [𝑏𝑏𝑑𝑑,𝑏𝑏𝑑𝑑,𝜑𝜑𝑑𝑑,𝜃𝜃11, 𝑠𝑠1,𝜃𝜃12,𝜃𝜃21, 𝑠𝑠2,𝜃𝜃22,𝜃𝜃31, 𝑠𝑠3,𝜃𝜃32]

��𝑞 = [𝑏𝑏��𝑑,𝑏𝑏��𝑑,𝜑𝜑��𝑑, ��𝜃11, ��𝑠1, ��𝜃12, ��𝜃21, ��𝑠2, ��𝜃22, ��𝜃31, ��𝑠3, ��𝜃32]

��𝑞𝑖𝑖𝑖𝑖𝑑𝑑 = [𝑏𝑏��𝑑,𝑏𝑏��𝑑,𝜑𝜑��𝑑]𝑎𝑎𝑖𝑖𝑑𝑑 ��𝑞𝑑𝑑𝑑𝑑𝑑𝑑 = [��𝜃11, ��𝑠1, ��𝜃12, ��𝜃21, ��𝑠2, ��𝜃22, ��𝜃31, ��𝑠3, ��𝜃32]

Equation 3-12 : 3RPR, Case 2, Variable sets

𝐶𝐶 =

⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡

𝑏𝑏𝑑𝑑 − 𝑋𝑋𝑑𝑑1𝑏𝑏𝑑𝑑 − 𝑌𝑌𝑑𝑑1

𝜑𝜑𝑑𝑑 +5𝜋𝜋6− 𝜃𝜃1


𝜑𝜑𝑑𝑑 +𝜋𝜋6− 𝜃𝜃2


𝜑𝜑𝑑𝑑 −𝜋𝜋2− 𝜃𝜃3 ⎦

⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤

Equation 3-13 : 3RPR, Case 2, Position constraints

𝐴𝐴 = �𝐼𝐼 −𝐽𝐽1 0 0𝐼𝐼 0 −𝐽𝐽2 0𝐼𝐼 0 0 −𝐽𝐽3

�

16 | P a g e

q𝑑𝑑𝑑𝑑𝑑𝑑 = q(4: 12), q𝑖𝑖𝑖𝑖𝑑𝑑 = q(1: 3)

𝐴𝐴𝑑𝑑𝑑𝑑𝑑𝑑 = 𝐴𝐴(: ,4: 12),𝐴𝐴𝑖𝑖𝑖𝑖𝑑𝑑 = 𝐴𝐴(: ,1: 3)

Equation 3-14 : 3RPR, Case 2, Velocity constraints

3.1.2.4. Case 3: ‘cut’ at Point C: 6 joint Variables and Task Variables

In this case, we express the points Ci in terms of x, y and φ in one representation and in terms of θi1

and si in the other representation.

States, n=9



𝑞𝑞 = [𝑏𝑏𝑑𝑑,𝑏𝑏𝑑𝑑,𝜑𝜑𝑑𝑑,𝜃𝜃11, 𝑠𝑠1,𝜃𝜃21, 𝑠𝑠2,𝜃𝜃31, 𝑠𝑠3]

��𝑞 = [𝑏𝑏��𝑑,𝑏𝑏��𝑑,𝜑𝜑��𝑑, ��𝜃11, ��𝑠1, ��𝜃21, ��𝑠2, ��𝜃31, ��𝑠3]

��𝑞𝑖𝑖𝑖𝑖𝑑𝑑 = [𝑏𝑏��𝑑,𝑏𝑏��𝑑,𝜑𝜑��𝑑]𝑎𝑎𝑖𝑖𝑑𝑑 ��𝑞𝑑𝑑𝑑𝑑𝑑𝑑 = [��𝜃11, ��𝑠1, ��𝜃21, ��𝑠2, ��𝜃31, ��𝑠3]

Equation 3-15 : 3RPR, Case 2, Variable sets

The coordinates of point Ci can be expressed in terms of joint space variables (Ciq) as well as in terms

of task space variables (Cix).

[𝐶𝐶1𝑏𝑏 𝐶𝐶2𝑏𝑏 𝐶𝐶3𝑏𝑏] = �cos(𝜑𝜑) − sin(𝜑𝜑) 𝑏𝑏𝑑𝑑sin(𝜑𝜑) cos(𝜑𝜑) 𝑏𝑏𝑑𝑑

0 0 1� ∙

⎣⎢⎢⎢⎡−

𝑙𝑙√3

𝑙𝑙2√3

𝑙𝑙2√3

0𝑙𝑙2

−𝑙𝑙2

1 1 1 ⎦⎥⎥⎥⎤

𝐶𝐶𝑖𝑖𝑞𝑞 = �𝑏𝑏𝑏𝑏𝑖𝑖 + (𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖) ∗ cos(𝜃𝜃𝑖𝑖1)𝑏𝑏𝑏𝑏𝑖𝑖 + (𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖) ∗ sin(𝜃𝜃𝑖𝑖1)

1�

On isolating the x and y coordinates and subtracting, we get a total of 6 constraints. The resulting

constraint matrix is

⎣⎢⎢⎢⎢⎢⎢⎡��𝐶1𝑏𝑏[1]− ��𝐶1𝑞𝑞[1]��𝐶1𝑏𝑏[2]− ��𝐶1𝑞𝑞[2]��𝐶2𝑏𝑏[1]− ��𝐶2𝑞𝑞[1]��𝐶2𝑏𝑏[2]− ��𝐶2𝑞𝑞[2]��𝐶3𝑏𝑏[1]− ��𝐶3𝑞𝑞[1]��𝐶3𝑏𝑏[2]− ��𝐶3𝑞𝑞[2]⎦

⎥⎥⎥⎥⎥⎥⎤

Where,

17 | P a g e

[��𝐶1𝑏𝑏 ��𝐶2𝑏𝑏 ��𝐶3𝑏𝑏] = �−sin(𝜑𝜑)��𝜑 −cos(𝜑𝜑)��𝜑 𝑏𝑏��𝑑cos(𝜑𝜑)��𝜑 −sin(𝜑𝜑)��𝜑 𝑏𝑏��𝑑

0 0 0� ∙

⎣⎢⎢⎢⎡−

𝑙𝑙√3

𝑙𝑙2√3

𝑙𝑙2√3

0𝑙𝑙2

−𝑙𝑙2

1 1 1 ⎦⎥⎥⎥⎤

��𝐶𝑖𝑖𝑞𝑞 = �−(𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖) ∗ sin(𝜃𝜃𝑖𝑖1) ��𝜃𝑖𝑖1 + (��𝑠𝑖𝑖) ∗ cos(𝜃𝜃𝑖𝑖1)

(𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖) ∗ cos(𝜃𝜃𝑖𝑖1)��𝜃𝑖𝑖1 + (��𝑠𝑖𝑖) ∗ sin(𝜃𝜃𝑖𝑖1)0

�

3.2. Dynamic Modeling (Lagrangian method)

Dynamic modeling is generally done using the Euler Lagrange method of derivation. The main

equation behind this approach is

𝜕𝜕𝜕𝜕𝜕𝜕�𝜕𝜕𝜕𝜕𝜕𝜕��𝑞� −

𝜕𝜕𝜕𝜕𝜕𝜕𝑞𝑞 =

𝜕𝜕𝜋𝜋𝜕𝜕��𝑞

Equation 3-16 - Lagrangian formulation equation

Where,

L = Lagrangian = T – V = (kinetic energy) – (potential energy)

t = time

q, q = state variables and their time derivatives

π = Total power input into the system

Next, we use the equation stated above for each of the state variables and assemble the resulting nine

equations in the standard form for expressing dynamic equations, i.e.

𝑴𝑴��𝒒 + 𝑽𝑽 = 𝑬𝑬𝑬𝑬 − 𝑨𝑨𝑻𝑻𝝀𝝀

Equation 3-17 : General dynamic equation for constrained systems

Where

M = Mass matrix

V = Force Vector

E = Power dissipation matrix

q = State variable space

𝑬𝑬 = Torque input

λ = Constraint forces

In this case,

18 | P a g e

𝑇𝑇𝑖𝑖 =12𝑚𝑚𝑖𝑖0��𝑏𝑖𝑖02 + ��𝑏𝑖𝑖02 � +

12𝑚𝑚𝑖𝑖1��𝑏𝑖𝑖12 + ��𝑏𝑖𝑖12 � +

12𝑚𝑚𝑖𝑖2��𝑏𝑖𝑖22 + ��𝑏𝑖𝑖22 � +

12𝐽𝐽𝑖𝑖1��𝜃𝑖𝑖12 +

12𝐽𝐽𝑖𝑖2��𝜃𝑖𝑖1 + ��𝜃𝑖𝑖2�

2

Vi = 0 (due to planar constraint and gravity perpendicular to plane)

𝜕𝜕𝑖𝑖 = 𝑇𝑇𝑖𝑖

𝜕𝜕 = �𝜕𝜕𝑖𝑖

3

𝑖𝑖=1

𝜋𝜋 = �𝜏𝜏𝑖𝑖 ∙ θi1

3

𝑖𝑖=1

Equation 3-18 : Dynamic terms related to EOM derivation for 3RPR

Where,

mi0 = mass of on first link ith RPR subsystem

mi1 = mass of slider on ith RPR subsystem

mi2 = mass of second link on ith RPR subsystem

Ji1 = moment of inertia of first link about z axis

Ji2 = moment of inertia of second link about z axis

To show the Euler-Lagrange formulation, only a part of the Lagrangian is shown so as to avoid long

equations and confusion.

𝜕𝜕𝑑𝑑𝑎𝑎𝑝𝑝𝜕𝜕 =m10l11

2 ��𝜃112

8+

12𝐽𝐽11��𝜃11

2

𝜋𝜋𝑑𝑑𝑎𝑎𝑝𝑝𝜕𝜕 = 𝜏𝜏1 ∙ ��𝜃11

𝜕𝜕𝜕𝜕𝑑𝑑𝑎𝑎𝑝𝑝𝜕𝜕𝜕𝜕��𝜃11

=m10l11

2 ��𝜃11

4+ 𝐽𝐽11��𝜃11

𝜕𝜕𝜕𝜕𝜕𝜕 �

𝜕𝜕𝜕𝜕𝑑𝑑𝑎𝑎𝑝𝑝𝜕𝜕𝜕𝜕��𝜃11

� =m10l11

2 ��𝜃11

4+ 𝐽𝐽11��𝜃11

𝜕𝜕𝜕𝜕𝑑𝑑𝑎𝑎𝑝𝑝𝜕𝜕𝜕𝜕𝜃𝜃11

= 0

𝑂𝑂𝑂𝑂𝑑𝑑𝑝𝑝𝑎𝑎𝑙𝑙𝑙𝑙 𝑑𝑑𝑎𝑎𝑝𝑝𝜕𝜕 𝑑𝑑𝑞𝑞𝑒𝑒𝑎𝑎𝜕𝜕𝑖𝑖𝑐𝑐𝑖𝑖 ∶ m10l11

2 ��𝜃11

4+ 𝐽𝐽11��𝜃11 = 𝜏𝜏1 ∙ ��𝜃11

Equation 3-19 : Euler Lagrange formulation for part of EOM of 3RPR

19 | P a g e

3.3. Gauss Divergence Theorem

3.3.1. Theorem Statement

Most simply put, the Gauss divergence theorem states that the sum of all sources minus the sum of all

sinks gives the net flow out of a region.

Formal statement: the outward flux of a vector field through a closed surface is equal to the volume

integral of the divergence on the region inside the surface.

Mathematically: Let V be a region in space with boundary dV. Then the volume integral of the

divergence of F over V and the surface integral of F over the boundary dV of V are related by

� (∇.𝐅𝐅)dV𝑉𝑉

= � 𝑭𝑭.𝑑𝑑𝒂𝒂𝑑𝑑𝑉𝑉

Equation 3-20 : Mathematical statement of Gauss divergence theorem The theorem can also be used for a planar area A bounded by a curve S, in which case the equation

simplifies to

� (∇.𝐅𝐅)da𝑆𝑆

= � 𝑭𝑭.𝒏𝒏�𝑑𝑑𝑠𝑠𝑑𝑑𝑆𝑆

Equation 3-21 : Mathematical statement for Gauss divergence Theorem (2D) 3.3.2. Application to Workspace Volume

For calculating the workspace volume using the Gauss divergence theorem, the problem statement is:

Given the curves/surfaces bounding the workspace, find the enclosed area/volume of the 2D/3D

workspace.

To do this, the function F is set so that ∇.𝐅𝐅 = 𝟏𝟏 so that the left hand of Equation 3-21 simplifies to the

workspace area. Since the bounding curves of the workspace are known parametrically, the right side of

the equation is easily solved as shown below for the circular arc and the line. For any curve, we select

𝐹𝐹 =𝑏𝑏𝑖𝑖 + 𝑏𝑏𝑗𝑗

2⇒ ∇.𝐅𝐅 =

𝜕𝜕𝐹𝐹𝜕𝜕𝑏𝑏 ∙ ��𝑖+

𝜕𝜕𝐹𝐹𝜕𝜕𝑏𝑏 ∙ ��𝑗 =

12 +

12 = 1 ⇒ � (∇.𝐅𝐅)da

𝑆𝑆= � da

𝑆𝑆= 𝐴𝐴𝑝𝑝𝑑𝑑𝑎𝑎

Equation 3-22 : Gauss divergence theorem adaptation for area measurement (LHS)

The derivations for the equations of area under circular and linear curves using the Gauss divergence

theorem are covered in 11.3.

20 | P a g e

3.3.2.1. Example - 3-RPR mechanism

We seek to find the constant orientation workspace of the 3-RPR, i.e., the area reachable by the end-

effector while keeping the orientation of the central platform constant.

The method for finding the bounding curves for this constant orientation workspace area is discussed

in 4.1.4. Following this method of geometrical constraints, we find the following series of curves bounding

the constant orientation workspace (for φ=0) for the 3-RPR.

Figure 5 - Constant Orientation Workspace of 3RPR (φ =0)

The 3-RPR workspace is simply the intersection of the three individual workspaces whose inner and

outer circles are represented as dark and thin dotted lines. The thick solid lines (red), represent the

boundaries of the intersection area(s) of the individual workspaces. The area of the bounded workspace is

calculated by cycling through all the bounding curves while applying Equation 11-1 to each one. In this

case, the computed area of 22.6035 units2 is reasonably close to the computed area of 22.54 units2

obtained using a brute force method with inverse position kinematics at each point. This is justified further

by overlaying the output from the two methods.

-40 -30 -20 -10 0 10 20 30 40

-30

-20

-10

0

10

20

30

21 | P a g e

Figure 6 - Constant Orientation Workspace of 3RPR (phi=0) using brute force method (dark black patch) and Gauss divergence method (thick red solid line)

3.4. Simulink/Simmechanics

Simmechanics is a toolbox with Matlab that allows block diagram modeling and simulation of rigid body

systems. Instead of deriving and programming equations, one can use this multi-body simulation tool to

build a model composed of bodies, joints, constraints, and force elements that reflects the structure of the

system. An automatically generated 3-D animation helps visualize the system dynamics. Models complete

with mass, inertia, constraint, and 3-D geometry can be automatically imported from several CAD systems.

3.5. MapleSim

Maple is a computation software with easy and efficient symbolic computation built into it. It is used to

programmatically create symbolic equations of motion for several mechanisms during this thesis. Maplesim

is a toolbox built on top of Maple programming interface to allow for block diagram modeling and simulation

of electrical, hydraulic, rigid multi-body systems etc. The added advantage is that one can easily extract the

underlying equations of motion from the model by attaching a Maple sheet to the model. It has been mainly

-40 -30 -20 -10 0 10 20 30 40

-30

-20

-10

0

10

20

30

22 | P a g e

used to validate the equations of motion generated by hand as well as validation of forward dynamic

simulation results.

In the analytical approach, we discuss the following staged solution process:

(1) Idealize the problem by making appropriate assumptions, such as its inertia is like that of an

idealized rod.

(2) Draw the free body diagram of the simplified model.

(3) Develop the appropriate governing equation of motions (EOM) using Newton’s laws of motion

as well as using Lagrangian method.

(4) Solve the EOM to obtain the desired solution.

In the MapleSim approach, the simplified model is converted into a block based model as shown in

Figure 7(b). The model is then simulated in order to visualize its motion and extract the time history of

important parameters. Finally, the EOM are extracted by attaching a Maple sheet to the model and

exporting the dynamic matrices to Matlab for further analysis and simulation.

(a) (b)

23 | P a g e

(c)

Figure 7 - (a) Simple pendulum problem; (b) modeled in MapleSim environment; (c) generated EOMs, plot of results from forward dynamic simulation, and visualization of the pendulum in motion.

4. Workspace Analysis

The general method for workspace computation is a brute-force search using an inverse position

kinematics routine at each point. However, this approach is inefficient because it leads to lost workspace if

the initial grid selection does not cover the entire workspace and lost time and efficiency if the initial grid is

fairly larger than the workspace. Furthermore, it does not allow much insight into the actual constraints and

working of the hexapod. This method can be improved upon in the following ways:

1. Improve the efficiency of a single computation by replacing inverse position kinematics with Boolean

geometric checks

2. Decrease the number computations by using a different search method (like the recursive nearest

neighbor-search method)

3. Devise a geometric method of workspace computation that eliminates the grid-based approach

24 | P a g e

4.1. 3PRR

4.1.1. Nomenclature

As discussed, this model is a simplified version of the Hexapod. Its detailed schematics are as shown.

Figure 8: Simplified 2-D 3PRR system

Bi denote the vertices of the fixed equi-triangular base on which the prismatic joints Ai slide. αi denote

the inclinations of the three sides of the base with the inertial reference frame. Ci are the vertices of the

moving platform which is also assumed to be an equilateral triangle. AiCi are the fixed length links which

join the moving prismatic joints to the platform through two revolute joints. The origin of the absolute

reference frame is assumed to be fixed at O, the centroid of B1B2B3. The origin of a moving reference frame

is attached to point P, the centroid of C1C2C3. The references for the three prismatic joints are at the

centers of the three sides of the equi-triangular base. Φ is the angle of the central platform as expressed in

the inertial reference frame.

The joint variables are the prismatic joint positions (si) and revolute positions (θi1 and θi2) for all three

links (i = 1, 2, 3). The centroid of the central platform, which is also the endpoint of all the three individual

links, is designated as (xe,ye).

The geometrical information of the simplified model is given below:

25 | P a g e

𝐵𝐵1𝐵𝐵2 = 𝐵𝐵2𝐵𝐵3 = 𝐵𝐵3𝐵𝐵1 = 𝜕𝜕

𝐵𝐵1𝑂𝑂 = 𝐵𝐵2𝑂𝑂 = 𝐵𝐵3𝑂𝑂 =𝜕𝜕√3

𝐶𝐶1𝐶𝐶2 = 𝐶𝐶2𝐶𝐶3 = 𝐶𝐶3𝐶𝐶1 = 𝑙𝑙

𝐶𝐶1𝑃𝑃 = 𝐶𝐶2𝑃𝑃 = 𝐶𝐶3𝑃𝑃 =𝑙𝑙√3

𝐴𝐴𝑖𝑖𝐵𝐵𝑖𝑖 = 𝑙𝑙𝑖𝑖1; 𝑖𝑖 = 1,2,3

The generalized state variable space is:

q = [s1, θ11, θ12, s2, θ21, θ22, s3, θ31, θ32]

The minimal actuated variable space is:

z = [s1, s2, s3]

The cartesian output variable space is:

X = [xe,ye,φ]

4.1.2. Parametric Sweeps with Inverse Position Kinematics

In this method, we form a surface grid by taking an initial guess about a surface that might encompass

the entire workspace. The extent of this guess as well as the grid size are very important as they directly

affect the number of points created in the grid and hence the number of times the inverse position

kinematics routine is run. For best results, we iteratively form this initial guess to be fairly close to the actual

workspace so that the computation is complete in minimum time. For each inverse position kinematics

routine: (i) the three end points of the platform are calculated from the position of its centroid (a point on

the grid) and the constant orientation supplied; (ii) Each point is converted to the local reference frame and

the prismatic displacements are solved for by using the link lengths; (iii) For each prismatic slider, they are

checked for the prismatic joint limits (s < Lmax and s > Lmin). If this constraint is satisfied for all the 3 limbs,

then the point is considered to be a part of the workspace.

As a benchmark, we included the brute-force grid-based search to obtain a baseline. Although more

intelligent methods are available, the order of magnitude improvement achieved by exploiting the

underlying structure/geometry into the computation is the aspect we are seeking to highlight.

26 | P a g e

The brute-force search using inverse position kinematics routine gives the following results:

Figure 9 - Workspace of 3PRR with grid size 0.01

In the above figure, the black dotted lines represent the lines of action of the prismatic joints.

4.1.3. Recursive nearest neighbor search with Inverse Position Kinematics

In this approach, we change the way the grid is sampled to get the workspace. The main algorithm is

shown on the side. We choose an initial feasible point by choosing random points until a feasible point is

encountered. Next, we choose all the neighbors of this point and check for feasibility. All feasible neighbors

form the parent set for the next iteration. A check is also incorporated to avoid checking points already

encountered in the sampling.

-3 -2 -1 0 1 2 3

2.5

3

3.5

4

4.5

5

5.5

6

6.5

7

7.5

27 | P a g e

Figure 10 : Recursive nearest neighbor search algorithm

4.1.4. Exact workspace determination using geometric constraints

Although the improvements in the grid based method provide much faster results, any method with

grid-based searching can only be as accurate as the grid-spacing. Hence, we employ a method of

intersecting areas to find the workspace edges parametrically and then employ gauss divergence theorem

for workspace area calculation from these workspace edges.

This method simply uses the commutative property of vectors to convert the constant orientation

workspace of any parallel platform manipulator into a problem of intersection of displaced individual

workspaces.

We begin with the vector equation for the end point of the mechanism, i.e., the center of the platform.

𝑂𝑂𝑃𝑃 = 𝑂𝑂𝐴𝐴𝑖𝑖 + 𝐴𝐴𝑖𝑖𝐵𝐵𝑖𝑖 + 𝐵𝐵𝑖𝑖𝐶𝐶𝑖𝑖 + 𝐶𝐶𝑖𝑖𝑃𝑃

In case of a constant orientation workspace problem, the vector CiP remains constant throughout the

analysis. Also, the range of vector AiBi is known to be the range of the prismatic joint. Furthermore, the

28 | P a g e

length of vector BiCi is known to be constant. Hence, we can find a sweep of all the possible positions of

the vector AiCi in the form of a rectangle bounded on two sides by two semicircles. Thus, by knowing a

constant orientation, we can find all possible locations for the points Ci. This is illustrated in the figure

below.

Figure 11 - Individual PRR workspaces in 3PRR

In this figure, the thick dotted lines represent the prismatic joint motion limits and the thin solid lines

outside the figure represent the workspaces of the points Ci.

-5 -4 -3 -2 -1 0 1 2 3 4 5

-3

-2

-1

0

1

2

3

4

5

A1 B1

C1

A2

B2

C2

A3

B3

C3

P

29 | P a g e

Next, we just add the vector CiP beforehand in all the PRR linkages to shift the workspaces by certain

amounts. The resulting figure is shown below.

Figure 12 - Shifted individual PRR workspaces for 3PRR

Thus, by intersecting these 3 shifted areas, we can find the constant orientation workspace for the

3PRR. This fact can be verified by overlaying the areas with the workspace obtained using the brute-force

method.

Figure 13 - Overlay of 3PRR individual workspaces with brute-force method workspace

-6 -4 -2 0 2 4 60

1

2

3

4

5

6

7

8

9

10

-8 -6 -4 -2 0 2 4 6 8 100

1

2

3

4

5

6

7

8

9

10

30 | P a g e

In the above figure, the dark patch is the densely populated constant orientation workspace computed

using brute-force method and it matches reasonably with the common area between the three individual

workspaces.

The intersection can then be programmed for the Gauss-divergence theorem as discussed in 3.3.2.

Figure 14 - Workspace calculation using Gauss divergence - intersection Points

The workspace area as calculated by this method is 13.4748 units2, which is in close agreement with

the brute-force calculated volume of 13.4350 units2 (grid size=0.05) and 13.4792 units2 (grid size=0.01).

In order to improve the efficiency further, we can program a CAD package to compute the above stated

intersection automatically given the important geometric parameters. Since the workspace for a particular

orientation is 2D, we can use the third dimension, z, to represent the orientation angle φ and find the

complete continuous constant orientation workspace of the 3PRR in question.

The first step is to create the curves representing the changing direction of vector CiP as φ=z goes

from 0 to 2π. The next step is to sweep the areas created for the vectors AiCi over these curves and keep

the common volume. One such workspace is shown below.

-6 -4 -2 0 2 4 60

1

2

3

4

5

6

7

8

9

10

31 | P a g e

Figure 15 - 3PRR constant orientation workspace for all orientations

The top surface of the solid is the constant orientation workspace for φ=0 and can be compared

against the figure above. The workspace can be now cut at any given value of z= φ to give the constant

orientation workspace at that point and the surface area can also be easily measured.

4.2. Hexapod

Since the Hexapod has 6 DOF spatial manipulator, visualizing its complete 6-D workspace would be

difficult. To overcome this issue, such workspaces are classified into various groups [18] out of which two

types are commonly considered important and studied: (i) a constant orientation workspace is the 3D space

of points where the manipulator can reach while keeping its orientation fixed; (ii) a constant position

workspace is a set of orientations possible for the manipulator while keeping the position of the platform

center fixed [12].

4.2.1. Nomenclature

The Hexapod is simplified into a 6-PUS manipulator as shown below.

32 | P a g e

Figure 16 - Hexapod Structure and Reference frames

The origin, O, is fixed at the centroid of the equilateral triangle (shown in think solid line) formed by the

axes of action of the 6 prismatic joints (two prismatic joints in opposite directions along each axis). In the

figure, Ai‘s represent the starting points of the prismatic joints whose axes are aligned with the x-axis of the

reference frame at Ai. The point B represents the actual position of the prismatic slider as well as the

universal joint mounted atop the slider. The point C represents the endpoint of the central platform (shown

in gray patch) as well as the spherical joint connecting the central platform with the 6 legs, BC (shown in

thick dotted lines). The end-effector reference frame is fixed at point P.

The Hexapod is considered to be a combination of 3 Two Link Subsystems (TLS) connected to a

central platform. One TLS is shown the figure below.

Figure 17 : Hexapod TLS

We choose a Hexapod with the following parameters for the workspace evaluation:

TABLE 1 : HEXAPOD AI REFERENCE FRAME POSITIONS

i αi(deg) [ bxi, byi, bzi] 1 0 [ 0,−𝜕𝜕 2√3, 0]⁄ 2 120 [ 𝜕𝜕/4, 𝜕𝜕 4√3, 0]⁄ 3 240 [−𝜕𝜕/4, 𝜕𝜕 4√3, 0]⁄

OA1

A2

A3 B

CP

OA1

B

CP

33 | P a g e

4.2.2. Parametric Sweeps with Inverse Position Kinematics

In this method, we form a volume grid by taking an initial guess about a volume that might encompass

the entire workspace. The extent of this guess as well as the grid size are very important as they directly

affect the number of points created in the grid and hence the number of times the inverse position

kinematics routine is run. For best results, we iteratively form this initial guess to be fairly close to the actual

workspace so that the computation is complete in minimum time. For each inverse position kinematics

routine: (i) the three end points of the platform are calculated from the position of its centroid (a point on

the grid) and the constant orientation supplied; (ii) Each point is converted to the local reference frame and

the prismatic displacements are solved for by using the link lengths; (iii) For each prismatic slider, they are

checked for the prismatic joint limits (s < Lmax and s > Lmin). If this constraint is satisfied for all the 6 limbs,

then the point is considered to be a part of the workspace.

The brute-force search using inverse position kinematics routine gives the following results:

Figure 18 - Hexapod workspace for [0 0 0] orientation using brute force method with inverse position kinematics routine

The surfaces are obtained by finding the minimum and maximum feasible z positions for each [x y]

combination and plotting them.

34 | P a g e

4.2.3. Parametric Sweeps with Geometric checks

We can also use geometric checks to accelerate the workspace determination process to improve on

overall computation time for the grid-search method. To this end, the loop closure equation for each limb of

the platform can be written to formulate the position level constraints as:

𝑂𝑂𝑃𝑃�� + 𝑃𝑃𝐶𝐶�� = 𝑂𝑂𝐴𝐴�� + 𝐴𝐴𝐶𝐶��

Equation 4-1 : Basic Vector loop closure Equation for Hexapod

where, PC - end points of platform relative to its center, OA

- location of local coordinate systems for

prismatic joints with respect to O, AC - the values we evaluate for this analysis. All the vectors are

expressed in the global reference frame O.

Figure 19 – Vector diagram for Geometry-based checks on Hexapod

Now we attempt to find the maximum and minimum values for the magnitude of AC using geometric

constraints. For the purpose of this analysis, we assume that all the three pairs of legs have lengths l1, l2

and l3 but the legs in the same subsystem (as in Figure 17 for TLS) are equal in length. This is basically

enforced to eliminate any singular configurations in the hexapod resulting from any of the limbs becoming

perpendicular to their corresponding prismatic axes. Secondly, this assumption simplifies the kinematic

problem.

Our main aim here is to find maximum and minimum values for the magnitude of vector AC so that we

can determine the feasibility of a point by checking if it is within the calculated range for all the three

subsystems. Considering the ith subsystem (i=1, 2, 3), the maximum value for AC will be achieved when the

35 | P a g e

minimum prismatic displacement is reached and the minimum value for AC will be reached when the

maximum prismatic displacement is reached. Note that xlocal is the x-coordinate of C expressed in the local

reference frame, Ai of the TLS in consideration. This is shown in Figure 20 and Figure 21.

From the figures, we can directly infer that the values for maximum (rmax) and minimum (rmin) lengths for

AC are:

𝑝𝑝𝑚𝑚𝑖𝑖𝑖𝑖2 = 𝑙𝑙12 − (𝜕𝜕𝑚𝑚𝑎𝑎𝑏𝑏 − 𝑏𝑏𝑙𝑙𝑐𝑐𝑐𝑐𝑎𝑎𝑙𝑙 )2 + 𝑏𝑏𝑙𝑙𝑐𝑐𝑐𝑐𝑎𝑎𝑙𝑙2

𝑝𝑝𝑚𝑚𝑎𝑎𝑏𝑏2 = 𝑙𝑙12 − (𝜕𝜕𝑚𝑚𝑖𝑖𝑖𝑖 + 𝑏𝑏𝑙𝑙𝑐𝑐𝑐𝑐𝑎𝑎𝑙𝑙 )2 + 𝑏𝑏𝑙𝑙𝑐𝑐𝑐𝑐𝑎𝑎𝑙𝑙2

Equation 4-2 : rmax and rmin values for geometric checks method

By checking if the actual distance from each local reference frame is between these values, we can

determine if the point in consideration is reachable by the corresponding TLS.

Figure 20 – Geometric check method - rmin

Figure 21 - Geometric check method - rmax

36 | P a g e

Figure 22 - Hexapod workspace for [0 0 0] orientation using brute force method with geometric checks

Noting that the number of feasible points in this case is the same as that in the previous case and on

comparison with the shape in Figure 19, we can infer that the geometric check is identical to the inverse

position kinematics for purposes of workspace determination.

4.2.4. Recursive nearest neighbor search using Inverse Position Kinematics

As suggested in method 2, we can change the method for the sweep from parametric to a more

efficient method. One such example is the recursive nearest neighbor-search. We choose random points

from the workspace till a feasible point is reached and then “explode” outwards from that point in a tree-

structure to reveal the entire workspace.

Noting that the number of points is equal to the previous two methods, we can infer that the method

gives accurate results. This method is found to be better than method 1 but could be improved by replacing

the inverse position kinematics routine with a geometric check routine.

4.2.5. Recursive nearest neighbor search using Geometric checks

This method takes both suggestions 1 and 2 into consideration for achieving the best results among

the methods discussed so far.

It is found to be better than methods 2 and 3. However, the computation time for all the methods

discussed above increases drastically as the grid size is reduced.

37 | P a g e

Now, in order to perform a comparative study between the different methods, we use the common

computing platform with the following specification—Pentium dual-core 2.1Ghz processor with 4GB RAM

and Windows 7 Home Premium. The corresponding CPU times required to complete the workspace

computation are documented in Table 2. Note that this performance study is only intended to give a rough

estimate of the computation cost.

(a) (b)

Figure 23 : Comparative study of all grid search methods (a) log-log graph of implementation time w.r.t. grid size (b) log-log graph of % Volume error w.r.t. grid size

The following table outlines the comparative efficiencies of all grid-search methods.

TABLE 2 : COMPARATIVE EFFICIENCIES OF ALL GRID SEARCH METHODS FOR CONSTANT ORIENTATION WORKSPACE OF HEXAPOD AT GRID SIZE =0.0625

Method bruteF InvPosKin

Nearest neighbor InvPosKin

bruteF GeomCheck

Nearest neighbor GeomCheck

Time 13139.78 610.31 91.88 66.78 bruteF_InvPosKin 13139.78 0 95.36 99.30 99.49 Nearest neighbor_InvPosKin 610.31 0 84.95 89.06 bruteF_GeomCheck 91.88 0 27.32 Nearest neighbor_GeomCheck 66.78 0

-2.5 -2 -1.5 -1-2

-1

0

1

2

3

4

5

log10(Grid size)

log1

0(Im

plem

enta

tion

Tim

e)

bruteF_InvPosKinbruteF_GeomCheckTree_InvPosKinTree_GeomCheck

-2.5 -2 -1.5 -1-5

-4.5

-4

-3.5

-3

-2.5

-2

-1.5

-1

log10(Grid size)

log1

0(%

Wor

kspa

ce V

olum

e E

rror

)

bruteF_InvPosKinbruteF_GeomCheckTree_InvPosKinTree_GeomCheck

38 | P a g e

4.2.6. Exact Workspace determination using Geometric constraints

As in the earlier case, we begin with the geometric-constraints derived from the loop-closure equations.

Given the vector loop closure for a single TLS:

𝑂𝑂𝑃𝑃�� + 𝑃𝑃𝐶𝐶�� = 𝑂𝑂𝐴𝐴�� + 𝐴𝐴𝐵𝐵�� + 𝐵𝐵𝐶𝐶��

Equation 4-3 : Vector loop closure Equation for Geometry constraints modeling of Hexapod

For the forward position kinematics problem, given values for the actuated prismatic joints, OA

and

PC

are known and hence Equation 4-3 can be rewritten as:

𝑂𝑂𝑃𝑃�� = 𝑂𝑂𝐴𝐴�� + 𝐴𝐴𝐵𝐵�� + 𝐵𝐵𝐶𝐶�� − 𝑃𝑃𝐶𝐶��

𝑂𝑂𝐴𝐴1�� + 𝐴𝐴𝐵𝐵1�� + 𝐵𝐵𝐶𝐶1�� − 𝑃𝑃𝐶𝐶1�� = 𝑂𝑂𝐴𝐴2�� + 𝐴𝐴𝐵𝐵2�� + 𝐵𝐵𝐶𝐶2�� − 𝑃𝑃𝐶𝐶2��

Equation 4-4 : Rearranged vector loop closure equation for Geometry constraints modeling of Hexapod

As can be seen, all the vectors except 1 1BC

and 2 2B C

are known and can be substituted for. Also, the

magnitude of the unknown vectors is known (link lengths). Hence, we can reduce the problem of finding the

end-effector position to a problem of intersecting spheres. This is illustrated graphically in Figure 24 which

shows the original loop closure equation followed by Equation 4-4.

Figure 24 : Vector loop for Exact workspace using geometric constraints

In Figure 24, the dots along the line parallel to a prismatic joint axis through Ai represent the prismatic

joint limits (vectors shifted due to change in order of vector addition). Hence, according to Figure 24, the

link is at the maximum displacement of the right prismatic joint. Hence, the workspace can now be

calculated by cycling through the range of prismatic motions. We notice here that in order to any point

along the workspace boundaries, at least one of the prismatic joints must have reached their motion limits.

Hence, any surface in the entire workspace should lie along one of the limit spheres with a prismatic limit

point as its center and the link length as its radius. By the same token, any edge of the workspace

39 | P a g e

(intersection of two spherical surfaces) would be along the intersection of two spheres such that limit

constraints on any two prismatic sliders are active simultaneously. Similarly, any intersection point of the

workspace (intersection of two curves) would be the intersection of three spheres, or at those points, the

limit constraints for three sliders are active simultaneously.

To find the workspace, we start with the intersection points between different spherical surfaces. We

note that there are a total of 12 limit points for the 6 prismatic joints. Considering that any prismatic joint can

only be at one of the two limits at the same instant, we eliminate two of the 12 initially available limit points

by assuming one prismatic joint at its limit. Thus, we find the total number of 3-sphere intersection points

possible by finding the total number of combinations possible from the formula

𝑖𝑖 = (12) ∗ (12 − 2) ∗ (12 − 2 − 2) = 960

Equation 4-5 : Possible combinations of three sphere intersection points for Hexapod constant orientation workspace

Next, we find the actual intersection points for all of these possibilities and check whether they

satisfy conditions for the other links to reach the point in consideration (whether the other 3 links assumed

not to be at the limits are feasibly placed to reach this point). This criterion’s implementation is similar to the

previous method. This decreases the number of points that we need to find from 960 to the actual number

of points on the workspace (18 in this particular example). The last step is to find the points with 2 common

spheres and draw curves between them along the intersection of the two common spheres. Finally, we find

the curves having a common sphere index and plot surfaces between them along the common sphere. The

resulting workspace envelope is obtained and is visually represented as in Figure 25.

Figure 25 – Exact Workspace using Geometric checks - 3D Workspace

40 | P a g e

Figure 26 : Comparison of workspace from grid-search method (Shaded) and Exact workspace method (Solid line)

To verify the validity of this approach, we eliminate the surface plots from Exact workspace method

and overlay the resulting line plot with the workspace plot from grid Search Method as shown in Figure 26.

As expected, the workspace envelope obtained by Exact workspace method is similar to, though more

accurate than the one obtained from the parameter sweep method. Furthermore, it can also be verified that

the differences are less than the grid size used in grid Search method (0.05 in this case) as shown in Table

3. We see that as the grid size decreases, the min/max X/Y/Z values from the grid-search method converge

to those found by using Exact Workspace method.

TABLE 3 : METHOD COMPARISON (ACCURACY)

Grid search method

(for different grid sizes)

Exact

workspace

method

Difference

0.1 (a) 0.05 (b) 0.025 (c) Exact (d) (d-b)

Min X -0.800 -0.850 -0.850 -0.875 -0.025

Min Y -0.900 -0.900 -0.900 -0.918 -0.018

Min Z 1.400 1.400 1.350 1.333 -0.017

Max X 0.800 0.800 0.800 0.797 -0.004

Max Y 0.700 0.750 0.750 0.775 0.025

Max Z 2.400 2.400 2.400 2.420 0.020

4.2.7. Using CAD modeling software to compute volumes

However, we notice that the above three approaches relied on the user’s ability to code part of the

computation in Matlab. An alternative, more graphical approach is to intersect the three separate feasible

regions of all three TLS’ and form a workspace, which is possible using a 3D CAD package like Pro-E or

Solidworks. It should be noted that the CAD package is being used to compute the intersection of

the spheres and not to perform the 3D detailed drawing of the parallel manipulator in question!

41 | P a g e

Figure 27 - 3D [0 0 0] orientation workspace by Exact Workspace method

Figure 28 - Solidworks representation of constant orientation workspace of Hexapod at [0 0 0] orientation

As further validation of the method, the 3D constant orientation workspace of the Hexapod is shown for

an orientation of [30 20 0] degrees.

Figure 29 : Hexapod at Orientation [30 20 0] degrees

OA1

A2

A3 B

C

P

42 | P a g e

Figure 30 : [30 20 0] orientation workspace using Exact workspace method (MATLAB)

Figure 31 : 3D Workspace by Exact workspace CAD method- ([α, β, γ]T = [30, 20, 0])

The advantages of developing an efficient CAD based system for constant orientation workspace

computation can be seen for doing parametric studies and optimization. For example, if only limited space

is available for placing the entire system, it places a limit on the base triangle length (L). Hence, a

parametric study to maximize the workspace volume for a constant orientation of [0 0 0] needs to be

conducted by varying the leg lengths and central platform size (characterized by l). We can easily do this

computation in ProE by using the multi-objective design optimization tool. Our study had the following

parameter variations:

TABLE 4 - PARAMETER SWEEP VALUES FOR CAD MODEL

Parameter Axis Min Max Resolution # Values

LA-Platform Length X 1.0 3.0 0.1 21

R1- Link Length Y 2.3 4.3 0.1 21

Volume Z 0.4 1.26 - 441

43 | P a g e

The program required approximately 42.5 seconds for a test run of 441 points (details shown in Table

4) thereby leading to a computation time of 0.0963 seconds/run, which is an order of magnitude better than

the best grid-search result for the same grid size. The results of this sweep were exported to a csv file and

plotted using MATLAB as shown in Figure 32.

Figure 32 : Workspace volume Optimization results from CAD model

4.3. Stewart Platform

This method can be applied to the Stewart Platform with minor changes to the CAD model.

Figure 33 - Comparison of Stewart Gough platform results with Yoshito paper [15]

We just need to change the equations for CP

and change the local reference frame locations to suit

that for the Stewart platform. Doing these minor changes, one can easily get the workspace for a Stewart

platform. To demonstrate this concept, we compare against Yoshito's results for the home orientation, [0, 0,

0]T using the parameters from that paper as shown in Figure 33.

12

3 2 3 4 50

0.5

1

1.5

Length of all 6 LinksPlatform Length

Wor

kspa

ce V

olum

e

44 | P a g e

5. Kinematic and Dynamic Analysis of 3-PRR

The nomenclature of the 3PRR has been covered in 4.1.1.


As discussed, the various parameters are:

αi : Inclination of base link i in global reference frame. i=1:3

si : Position of prismatic joint i

θi1 : Angle of rotation of first revolute joint in ith PRR Sub-System with respect to prismatic joint axis

θi2 : Angle of rotation of second revolute joint in ith PRR Sub-System with respect to first revolute joint

L : Length of side of equi-triangular base

li1: Length of first revolute link in ith PRR Sub-System

li2: Length of second revolute link in ith PRR Sub-System

φ : Orientation of platform in the global reference frame

We attempt to find the locations of the centers of mass of each body and also the endpoint of each link.

Center of masses of prismatic mass, first link and second link

𝑏𝑏𝑖𝑖0 = 𝑏𝑏𝑏𝑏𝑖𝑖 + 𝑠𝑠𝑖𝑖 ∙ cos(𝛼𝛼𝑖𝑖)

𝑏𝑏𝑖𝑖0 = 𝑏𝑏𝑏𝑏𝑖𝑖 + 𝑠𝑠𝑖𝑖 ∙ 𝑠𝑠𝑖𝑖𝑖𝑖(𝛼𝛼𝑖𝑖)

𝑏𝑏𝑖𝑖1 = 𝑏𝑏𝑏𝑏𝑖𝑖 + 𝑠𝑠𝑖𝑖 ∙ cos(𝛼𝛼𝑖𝑖) +𝑙𝑙𝑖𝑖12∙ cos(𝜃𝜃𝑖𝑖1 + 𝛼𝛼𝑖𝑖)

𝑏𝑏𝑖𝑖1 = 𝑏𝑏𝑏𝑏𝑖𝑖 + 𝑠𝑠𝑖𝑖 ∙ 𝑠𝑠𝑖𝑖𝑖𝑖(𝛼𝛼𝑖𝑖) +𝑙𝑙𝑖𝑖12∙ sin(𝜃𝜃𝑖𝑖1 + 𝛼𝛼𝑖𝑖)

𝑏𝑏𝑖𝑖2 = 𝑏𝑏𝑏𝑏𝑖𝑖 + 𝑠𝑠𝑖𝑖 ∙ cos(𝛼𝛼𝑖𝑖) + 𝑙𝑙𝑖𝑖1 ∙ cos(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1) +𝑙𝑙𝑖𝑖22∙ cos(𝛼𝛼𝑖𝑖+𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)

𝑏𝑏𝑖𝑖2 = 𝑏𝑏𝑏𝑏𝑖𝑖 + 𝑠𝑠𝑖𝑖 ∙ sin(𝛼𝛼𝑖𝑖) + 𝑙𝑙𝑖𝑖1 ∙ sin(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1) +𝑙𝑙𝑖𝑖22∙ sin(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)

Equation 5-1 : 3PRR – Position of link masses

Center of platform:

𝑏𝑏𝑖𝑖3 = 𝑏𝑏𝑏𝑏𝑖𝑖 + 𝑠𝑠𝑖𝑖 ∙ cos(𝛼𝛼𝑖𝑖) + 𝑙𝑙𝑖𝑖1 ∙ cos(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1) + 𝑙𝑙𝑖𝑖2 ∙ cos(𝛼𝛼𝑖𝑖+𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)

𝑏𝑏𝑖𝑖3 = 𝑏𝑏𝑏𝑏𝑖𝑖 + 𝑠𝑠𝑖𝑖 ∙ 𝑠𝑠𝑖𝑖𝑖𝑖(𝛼𝛼𝑖𝑖) + 𝑙𝑙𝑖𝑖1 ∙ sin(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1) + 𝑙𝑙𝑖𝑖2 ∙ sin(𝛼𝛼𝑖𝑖+𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)

Equation 5-2 : 3PRR - Position of Center of Platform

45 | P a g e

Now, differentiating the position level equations, we get

𝑏𝑏𝑖𝑖0 = 𝑠𝑠��𝑖 ∙ cos(𝛼𝛼𝑖𝑖)

𝑏𝑏𝑖𝑖0 = 𝑠𝑠��𝑖 ∙ 𝑠𝑠𝑖𝑖𝑖𝑖(𝛼𝛼𝑖𝑖)

𝑏𝑏𝑖𝑖1 = 𝑠𝑠��𝑖 ∙ cos(𝛼𝛼𝑖𝑖) −𝑙𝑙𝑖𝑖12∙ sin(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1) ∙ 𝜃𝜃𝑖𝑖1

𝑏𝑏𝑖𝑖1 = 𝑠𝑠��𝑖 ∙ sin(𝛼𝛼𝑖𝑖) +𝑙𝑙𝑖𝑖12∙ cos(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1) ∙ 𝜃𝜃𝑖𝑖1

𝑏𝑏𝑖𝑖2 = 𝑠𝑠��𝑖 ∙ cos(𝛼𝛼𝑖𝑖) − 𝑙𝑙𝑖𝑖1 ∙ sin(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1) ∙ 𝜃𝜃𝑖𝑖1 −𝑙𝑙𝑖𝑖22∙ sin(𝛼𝛼𝑖𝑖+𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2) ∙ (𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 )

𝑏𝑏𝑖𝑖2 = 𝑠𝑠��𝑖 ∙ sin(𝛼𝛼𝑖𝑖) + 𝑙𝑙𝑖𝑖1 ∙ cos(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1) ∙ 𝜃𝜃𝑖𝑖1 +𝑙𝑙𝑖𝑖22∙ cos(𝛼𝛼𝑖𝑖+𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2) ∙ (𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 )

𝑏𝑏𝑖𝑖3 = 𝑠𝑠��𝑖 ∙ cos(𝛼𝛼𝑖𝑖) − 𝑙𝑙𝑖𝑖1 ∙ sin(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1) ∙ 𝜃𝜃𝑖𝑖1 − 𝑙𝑙𝑖𝑖2 ∙ sin(𝛼𝛼𝑖𝑖+𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2) ∙ (𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 )

𝑏𝑏𝑖𝑖3 = 𝑠𝑠��𝑖 ∙ sin(𝛼𝛼𝑖𝑖) + 𝑙𝑙𝑖𝑖1 ∙ cos(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1) ∙ 𝜃𝜃𝑖𝑖1 + 𝑙𝑙𝑖𝑖2 ∙ cos(𝛼𝛼𝑖𝑖+𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2) ∙ (𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 )

Equation 5-3 : 3PRR – Velocities of all points in mechanism

However, we do not require the third differential equation because

𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 = (𝑖𝑖 − 1) ∙2𝜋𝜋3

+𝜋𝜋6

+ 𝜑𝜑

∴ ��𝜃𝑖𝑖1 + ��𝜃𝑖𝑖2 = ��𝜑

Equation 5-4 : 3PRR – angle relationships

Hence, writing the equations in the state-space form:

��𝑏��𝑏��𝜑� = �

cos(𝛼𝛼𝑖𝑖) −𝑙𝑙𝑖𝑖1 ∙ sin(𝜃𝜃𝑖𝑖1) − 𝑙𝑙𝑖𝑖2 ∙ sin(𝜃𝜃𝑖𝑖2) −𝑙𝑙𝑖𝑖2 ∙ sin(𝜃𝜃𝑖𝑖2)sin(𝛼𝛼𝑖𝑖) 𝑙𝑙𝑖𝑖1 ∙ cos(𝜃𝜃𝑖𝑖1) + 𝑙𝑙𝑖𝑖2 ∙ cos(𝜃𝜃𝑖𝑖2) 𝑙𝑙𝑖𝑖2 ∙ cos(𝜃𝜃𝑖𝑖2)

0 1 1� �𝑠𝑠��𝑖𝜃𝜃𝑖𝑖1𝜃𝜃𝑖𝑖2

�

𝑋𝑋��𝑑 = 𝐽𝐽1𝑞𝑞1 ⇒ 𝑋𝑋��𝑑 = 𝐽𝐽2𝑞𝑞2 = 𝐽𝐽3𝑞𝑞3

Equation 5-5 : 3PRR – Jacobian representation

From the above equations, we have 3 task space DOF’s and 9 joint space DOF’s. Therefore, we can

select 3 independent DOF’s from joint space. Since the prismatic joints are actuated, we select DOF’s 1, 4

and 7 as the independent DOF’s.

46 | P a g e

In order to express the dependent DOF’s in terms of independent DOF’s, we rewrite the above

equations in the form:

�𝐽𝐽1q1 − 𝐽𝐽2q2𝐽𝐽1𝑞𝑞1 − 𝐽𝐽3𝑞𝑞3

�6∗1

= �00�6∗1

�𝐽𝐽1 −𝐽𝐽2 0𝐽𝐽1 0 −𝐽𝐽3

�6∗9

�𝑞𝑞1𝑞𝑞2𝑞𝑞3

�9∗1

= �00�6∗1

Equation 5-6 - Overall kinematic Constraints

Now, we have a matrix representation wherein each column in the J matrix represents the contribution

of a particular state in the system. Hence, we isolate the 1st, 4th and 7th columns corresponding to the

independent degrees of freedom and find that,

𝐴𝐴𝑑𝑑𝑑𝑑𝑑𝑑 ∙

⎣⎢⎢⎢⎢⎢⎡𝜃𝜃11𝜃𝜃12𝜃𝜃21𝜃𝜃22𝜃𝜃31𝜃𝜃32 ⎦

⎥⎥⎥⎥⎥⎤

+ 𝐴𝐴𝑖𝑖𝑖𝑖𝑑𝑑 ∙ �𝑠𝑠1��𝑠2𝑠𝑠3

� = �00�6∗1

𝐴𝐴𝑑𝑑𝑑𝑑𝑑𝑑 ��𝑞𝑑𝑑𝑑𝑑𝑑𝑑 + 𝐴𝐴𝑖𝑖𝑖𝑖𝑑𝑑 ��𝑞𝑖𝑖𝑖𝑖𝑑𝑑 = 0

∴ ��𝑞𝑑𝑑𝑑𝑑𝑑𝑑 = 𝐶𝐶 ∙ ��𝑞𝑖𝑖𝑖𝑖𝑑𝑑 , 𝐶𝐶 = −𝐴𝐴𝑑𝑑𝑑𝑑𝑑𝑑 −1 ∙ 𝐴𝐴𝑖𝑖𝑖𝑖𝑑𝑑

Equation 5-7 - DOF wise Constraints

Now, we compute a matrix relating all DOF’s to the independent DOF’s

��𝑞𝑎𝑎𝑙𝑙𝑙𝑙 = 𝑆𝑆 ∙ ��𝑞𝑖𝑖𝑖𝑖𝑑𝑑

The final step is to compute the independent DOF’s in terms of the required end-effectors DOF’s. In

order to do that, we observe that,

𝑋𝑋��𝑑 = 𝐽𝐽1��𝑞1 𝑎𝑎𝑖𝑖𝑑𝑑 q1 = 𝑆𝑆(1: 3) ∙ ��𝑞𝑖𝑖𝑖𝑖𝑑𝑑 ⇒ 𝑞𝑞𝑖𝑖𝑖𝑖𝑑𝑑 = (𝐽𝐽1 ∙ 𝑆𝑆(1: 3))−1 ∙ 𝑋𝑋𝑑𝑑

Equation 5-8 - Independent DOF calculation

The inversion of the matrix is using a pseudo inverse since the matrix is not square.

While calculating the, ��𝑞𝑖𝑖𝑖𝑖𝑑𝑑 , we can also add a null-space component to accomplish secondary tasks.

The null-space matrix maybe calculated as

𝑁𝑁 = 𝐼𝐼 − (𝐽𝐽1 ∙ 𝑆𝑆(1: 3))−1 ∙ (𝐽𝐽1 ∙ 𝑆𝑆(1: 3))

Equation 5-9 - Null Space Component Calculation

Hence, given any state of the system, we can find the state derivatives.

47 | P a g e

5.2. Dynamic Analysis

5.2.1. Hand derivation

We attempt the dynamic modeling using the Euler Lagrange method discussed in section 3.2.

Using the equations for the positions and velocities of the center of mass for different bodies of the

system, we define the Kinetic Energy for the ith subsystem as

𝑇𝑇𝑖𝑖 =12𝑚𝑚𝑖𝑖0��𝑏𝑖𝑖02 + ��𝑏𝑖𝑖02 � +

12𝑚𝑚𝑖𝑖1��𝑏𝑖𝑖12 + ��𝑏𝑖𝑖12 � +

12𝑚𝑚𝑖𝑖2��𝑏𝑖𝑖22 + ��𝑏𝑖𝑖22 � +

12𝐽𝐽𝑖𝑖1��𝜃𝑖𝑖12 +

12𝐽𝐽𝑖𝑖2��𝜃𝑖𝑖1 + ��𝜃𝑖𝑖2�

2

Vi = 0 (due to planar constraint and gravity perpendicular to plane)

𝜕𝜕𝑖𝑖 = 𝑇𝑇𝑖𝑖

𝜕𝜕 = �𝜕𝜕𝑖𝑖

3

𝑖𝑖=1

Equation 5-10 : Hand derivation of 3PRR dynamic EOMs

Where

mi0 = mass of slider on ith PRR subsystem

mi1 = mass of first link on ith PRR subsystem

mi2 = mass of second link on ith PRR subsystem

Ji1 = moment of inertia of first link about z axis

Ji2 = moment of inertia of second link about z axis

Here, state variables, q = [s1, θ11, θ12, s2, θ21, θ22, s3, θ31, θ32]

In this case,

𝑀𝑀 = �𝑀𝑀1 0 00 𝑀𝑀2 00 0 𝑀𝑀3

�

𝑉𝑉 = �𝑉𝑉1𝑉𝑉2𝑉𝑉3

�

48 | P a g e

𝑋𝑋 =

⎣⎢⎢⎢⎢⎢⎢⎢⎡1 0 00 0 00 0 00 1 00 0 00 0 00 0 10 0 00 0 0⎦

⎥⎥⎥⎥⎥⎥⎥⎤

𝑀𝑀𝑖𝑖

=

⎣⎢⎢⎢⎢⎡ mi0 + mi1 + mi2 −

12

mi1li1si1 − mi2li1si1 −12

mi2li2si3 −12

mi2li2si3

−12

mi1li1si1 − mi2li1si1 −12

mi2li2si314

mi1li12 + mi2li1

2 +14

mi2li22 + Ji1 + Ji2 + mi2li1li2ci2 Ji2 +

14

mi2li22 +

12

mi2li1li2ci2

−12

mi2li2si3 Ji2 +14

mi2li22 +

12

mi2li1li2ci214

mi2li22 + Ji2 ⎦

⎥⎥⎥⎥⎤

𝑉𝑉𝑖𝑖 =

⎣⎢⎢⎢⎢⎡12

mi1li1θi12 ci1 + mi2li1θi1

2 ci1 +12

mi2li2θ112 ci3 + mi2li2θi1θi2ci3 +

12

mi2li2θi22 ci3

12

mi2li1li2θi2si2(θi2 + 2θi1);

−12

mi2li1li2θi12 si2 ⎦

⎥⎥⎥⎥⎤

Equation 5-11 : Dynamic matrices for the 3PRR (9 joint variables set)

where

si1=sin(θi1); si2=sin(θi2); si3=sin(θi1+θi2)

ci1=cos(θi1); ci2=cos(θi2); ci3=cos(θi1+θi2)

The E matrix indicates that the three forces applied to the system will be distributed to the 1st, 4th and

7th DOFs of the system, which are the three prismatic joints in the system.

5.2.2. Maple derivation

Similar to hand derivation, we can do a symbolic analysis using Maple. The following are the basic

steps:

1. Create variable set and define them as functions of time.

2. Define the positions of all centers of mass and differentiate them to get their velocities.

3. Find the total kinetic and potential energies in the system and the Lagrangian as their difference.

4. Create Euler Lagrange procedure within Maple according to the Euler Lagrange equation and apply

on obtained Lagrangian with all state variables to compute the dynamic equations of the model.

5. Isolate the double derivative terms from the Euler Lagrange equations to form the mass matrix.

49 | P a g e

6. Subtract out the double derivative terms from the Euler Lagrange equations to form the force vector.

7. Differentiate the position constraints in the system to get the velocity constraints and isolate the

single derivative terms from these differentiated constraints to form the constraint matrix.

8. Use proper substitution and export all matrices and vectors to txt/csv formats for use in Matlab.

9. Import the matrices into Maplesim and compare them with matrices obtained automatically from

Maplesim to get the differences. Verify that the obtained differences are zero.

A detailed description of Maple Code is available in 11.4.

On comparison of the txt files thus created, we find that the mass matrix, constraint matrix and force

vectors are a perfect term-by-term match.

5.2.3. MapleSim Modeling

MapleSim is a multi-domain modeling and simulation tool. It does symbolic computation of the

equations of motion and provides them in various forms like the state-space model or the differential

equations form. Additionally, it can be used to analyze a system directly or export the model to a Simulink

model for further analysis. The main features included in the Maplesim model were:

• Three PRR sub-systems (with two links each) were developed and connected to each other

via rotated reference frames. Gravity was in the negative z-direction.

• Inside each PRR system, we have a prismatic joint. Following that, we have two revolute joints

and links representing the 2R’s.

• The reference frames in the main are as follows:

Parameter Values MapleSim Parameter

Angles of rotation of base links in global

reference frame �0

2𝜋𝜋3

4𝜋𝜋3�

Rotation matrix angles for ref. frames

before PRR subsystems

Base locations for 3 prismatic joints

⎣⎢⎢⎡ 0

𝜕𝜕4

−𝜕𝜕4

−𝜕𝜕√3

𝜕𝜕2√3

−𝜕𝜕2√3⎦

⎥⎥⎤

Translation vectors for ref. frames before

PRR subsystems

Orientation of each PRR end link with

respect to second PRR end

30◦+(i-1)*120◦,

i=1:3

Rotation matrix angles for ref. frames after

PRR subsystems

Equation 5-12 - Reference frames and parameters

50 | P a g e

The resulting system is shown in the following figure:

Figure 34 : Maplesim model for Hexapod

After this step, we run a simulation to check the modeling. The simulation results are then compared to

the forward simulation results from Matlab and Simmechanics. This is done in Figure 38.

The next step is to link this model to Maple and extract the underlying equations. Using ‘BuildEQs’

command, we build the equations and extract the mass and constraint matrices and the force vector. Due

to the length of the equations, they have been included in the appendix.

We see that there are only seven variables in the formulation. This is because MapleSim tries to

compute the dynamic equations with the least amount of time and variables. We can change this default

set and force MapleSim to include all the nine variables that we have chosen for prior analysis by giving

initial conditions on them. The new mass and constraint matrices and force vector are exactly matching the

ones derived with hand calculation and are shown in Equation 5-11.

As we can see, the mass matrix and force vector match term by term. This validates both the hand

derivation as well as the MapleSim software results. The constraint matrices still don’t match as they have

been created from Position constraints expressed in different reference frames. Both of them are accurate.

51 | P a g e

5.3. Matlab Kinematic Simulation

Before doing a dynamic simulation, a kinematic simulation was attempted. We use the constraint

matrix created from Maple derivation for this purpose.

5.3.1. Calculation of Initial Pose

Now, we should be done if we can calculate the initial pose of the system.

In order to do that, we start with the required starting point and rotation of the central platform and

calculate its end points. The third link angle of each link is related to the platform angle φ as explained

earlier during kinematic analysis. Then, we calculate the prismatic movement and angle of the first link by

setting up a Newton Raphson method. The condition is that the same point lies on a circle (with its center at

the end point of the platform and radius equal to the link’s length) and on the line joining the two base

points.

5.3.2. Final Simulation Run

Hence, we successfully complete the kinematic analysis of the system. For running the analysis, we

can use either fixed time step solvers (like ode4) or variable time step solvers (like ode45) to get the

following result:

The blue, green and red links represent the 3 different PRR sub-systems. The magenta link represents

the central platform. The bold black curve represents the ellipse that has been traced till this point. The

dotted black lines represent the base prismatic joints.

-2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5

3

52 | P a g e

5.4. Simmechanics Forward Dynamics Simulation Comparison

As an additional step to verify that the modeling is correct, we try to simulate the same models with

Simmechanics, a mechanics toolbox for Simulink and Matlab. The following model was created in

Simmechanics to duplicate the MapleSim model.

Figure 35 - Initial Pose of 3PRR for dynamic Simulation

Figure 36 - 3PRR Forward dynamics setup - Simmechanics

-3 -2 -1 0 1 2 3-3

-2

-1

0

1

2

3

53 | P a g e

Figure 37 - 3PRR forward dynamics model - Simmechanics

After computing the dynamic matrices, we use the method of Force projection to create the forward

dynamics routine in Matlab. The EOM for a constrained manipulator are expressed as


where, λ is the vector of constraint forces. Combining this equation with the acceleration constraints

��𝐴 ∙ ��𝑞 + 𝐴𝐴 ∙ ��𝑞 = 0

we can write,

�𝑀𝑀 𝐴𝐴𝑇𝑇𝐴𝐴 0

� ��𝑞𝜆𝜆� = �𝑋𝑋𝜏𝜏 − 𝑉𝑉−��𝐴��𝑞 � ⇒ ��𝑞𝜆𝜆� = �𝑀𝑀 𝐴𝐴𝑇𝑇

𝐴𝐴 0�−1�𝑋𝑋𝜏𝜏 − 𝑉𝑉−��𝐴��𝑞 �

Equation 13 : Forward dynamic simulation equation

Hence, given the state positions and velocities, we can find the state accelerations.

On comparing the results, we find close matching for the calculated positions and velocities of all

states between the Matlab and Simmechanics forward dynamics simulations. Hence, we can infer that our

Matlab code is functional.

54 | P a g e

On comparing the results with MapleSim, we find that the Simmechanics and MapleSim results are a

satisfactory match. The high “error” in the first graph is attributed to the angular positions since for any

angle θ, θ=θ+2π. As a final step before real time implementation, we program this Simmechanics model as

our forward dynamics simulator (or system model) and apply our control techniques on it.

(a) (b)

(c) (d) Figure 38 - Maplesim/Matlab Forward dynamics comparison

(a) Difference in state positions calculated (b) Difference in state positions calculated (zoomed in) (c) Difference in state velocities calculated (d) Torques supplied

5.5. Simmechanics Control Dynamics Simulation

In this step, we combine the inverse dynamics from Matlab with the forward dynamics from

Simmechanics to create a dynamic control simulation.

The control torque is determined by projecting onto the feasible motion space.

The EOM for a constrained manipulator are expressed as

0 5 10 15 20 25 30 35 40-1

0

1

2

3

4

5

6

7

s1dth11dth12ds2dth21dth22ds3dth31dth32d

0 5 10 15 20 25 30 35 40

-5

-4

-3

-2

-1

0

1

2

3

4x 10

-4

s1dth11dth12ds2dth21dth22ds3dth31dth32d

0 5 10 15 20 25 30 35 40-6

-4

-2

0

2

4

6x 10

-4

s1th11th12s2th21th22s3th31th32

0 5 10 15 20 25 30 35 40-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

Tau1Tau2Tau3

55 | P a g e


where, λ is the vector of constraint forces.

In order to get rid of the last term with unknown λ, we use the fact that S is the null space of A and pre-

multiply the entire equation with ST to get

𝑺𝑺𝑻𝑻𝑴𝑴��𝒒 + 𝑺𝑺𝑻𝑻𝑽𝑽 = 𝑺𝑺𝑻𝑻𝑬𝑬𝑬𝑬 − 𝑺𝑺𝑻𝑻𝑨𝑨𝑻𝑻𝝀𝝀 = 𝑺𝑺𝑻𝑻𝑬𝑬𝑬𝑬

(∵ 𝑨𝑨𝑺𝑺 = 𝟎𝟎 ⇒ 𝑺𝑺𝑻𝑻𝑨𝑨𝑻𝑻 = 𝟎𝟎)

Equation 14 : Force projection method for Inverse dynamics

Hence, the torque may be calculated as

𝑬𝑬 = (𝑺𝑺𝑻𝑻𝑬𝑬)(𝑺𝑺𝑻𝑻𝑴𝑴��𝒒 + 𝑺𝑺𝑻𝑻𝑽𝑽)

Equation 15 : Inverse dynamic simulation equation

In order to converge the initial errors in position/velocities (if any), we use PD control along with this

model-based control scheme.

𝑬𝑬 = (𝑺𝑺𝑻𝑻𝑬𝑬)−𝟏𝟏(𝑺𝑺𝑻𝑻𝑴𝑴��𝒒 + 𝑺𝑺𝑻𝑻𝑽𝑽) + 𝒌𝒌𝒑𝒑 ∙ (𝒛𝒛𝒅𝒅𝒅𝒅𝒅𝒅 − 𝒛𝒛𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂) + 𝒌𝒌𝒅𝒅 ∙ (��𝒛𝒅𝒅𝒅𝒅𝒅𝒅 − ��𝒛𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂)

Equation 16 : Inverse dynamic simulation overall equation

Where z represents the prismatic joint positions/velocities.

Figure 39 - 3PRR Control dynamics setup - Simmechanics

56 | P a g e

The motion profiles required are sine waves of different magnitudes and frequencies and the following

are the results for a 50 seconds simulation.

(a) (b)

(c) (d) Figure 40 - Maplesim/Matlab control dynamics simulation

(a) Difference in expected and attained prismatic joint positions (b) difference in expected and attained prismatic joint velocities (c) Expected prismatic joint positions (d) Torques supplied

0 5 10 15 20 25 30 35 40 45 50-0.025

-0.02

-0.015

-0.01

-0.005

0

0.005

0.01

0.015

Pos_Error_s1

Pos_Error_s2Pos_Error_s3

0 5 10 15 20 25 30 35 40 45 50-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

Vel_Error_s1

Vel_Error_s2Vel_Error_s3

0 5 10 15 20 25 30 35 40 45 50-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

Pos_s1

Pos_s2Pos_s3

0 5 10 15 20 25 30 35 40 45 50-25

-20

-15

-10

-5

0

5

10

15

Force_s1

Force_s2Force_s3

57 | P a g e

6. Kinematic and Dynamic Analysis and Control of Hexapod

The nomenclature of the Hexapod is covered in 4.2.1.


As discussed in kinematic analysis example in 3.1.2, there are various choices of variable spaces for

the kinematic and dynamic analysis of the hexapod.

Since Maplesim forces those variables into the variable set whose initial conditions are specified,

various possibilities were easily evaluated using the same hexapod model in Maplesim with different sets of

imposed initial conditions.

The first set was with all the prismatic joints included in the initial conditions, which led to a 21 variable

workspace.

𝑞𝑞 = [𝑠𝑠1, 𝑠𝑠2, 𝑠𝑠3, 𝑠𝑠4, 𝑠𝑠5, 𝑠𝑠6,𝑎𝑎𝑏𝑏,𝑎𝑎𝑏𝑏,𝑎𝑎𝑎𝑎,𝑎𝑎1,𝑏𝑏1,𝑎𝑎2,𝑏𝑏2,𝑎𝑎3, 𝑏𝑏3,𝑎𝑎4,𝑏𝑏4,𝑎𝑎5, 𝑏𝑏5,𝑎𝑎6,𝑏𝑏6]

𝑞𝑞𝑑𝑑𝑑𝑑𝑑𝑑 = [𝑎𝑎𝑏𝑏,𝑎𝑎𝑏𝑏,𝑎𝑎𝑎𝑎,𝑎𝑎1, 𝑏𝑏1,𝑎𝑎2,𝑏𝑏2,𝑎𝑎3, 𝑏𝑏3,𝑎𝑎4,𝑏𝑏4,𝑎𝑎5, 𝑏𝑏5,𝑎𝑎6,𝑏𝑏6],𝑞𝑞𝑖𝑖𝑖𝑖𝑑𝑑 = [𝑠𝑠1, 𝑠𝑠2, 𝑠𝑠3, 𝑠𝑠4, 𝑠𝑠5, 𝑠𝑠6]

The dynamic matrices for this configuration can be found in 11.6.2.

The second set was with all the prismatic joints and the end effector's position included, which led to a

24 variable analysis. However, because of the way Maplesim calculates the mass matrix, this condition set

led to some zero rows and columns in the evaluated mass matrix, which made it non-invertible. Hence, this

approach was not pursued further in Maplesim. However, Maple programming for the same was easily

done by adapting the 21-variable formulation above and removing the equations relating [xe, ye, ze] to the

prismatic displacements and platform orientation.

𝑞𝑞 = [𝑏𝑏𝑑𝑑,𝑏𝑏𝑑𝑑, 𝑎𝑎𝑑𝑑, 𝑠𝑠1, 𝑠𝑠2, 𝑠𝑠3, 𝑠𝑠4, 𝑠𝑠5, 𝑠𝑠6,𝑎𝑎𝑏𝑏,𝑎𝑎𝑏𝑏,𝑎𝑎𝑎𝑎,𝑎𝑎1, 𝑏𝑏1,𝑎𝑎2, 𝑏𝑏2,𝑎𝑎3, 𝑏𝑏3,𝑎𝑎4, 𝑏𝑏4,𝑎𝑎5,𝑏𝑏5,𝑎𝑎6, 𝑏𝑏6]

𝑞𝑞𝑑𝑑𝑑𝑑𝑑𝑑 = [𝑏𝑏𝑑𝑑, 𝑏𝑏𝑑𝑑, 𝑎𝑎𝑑𝑑,𝑎𝑎𝑏𝑏,𝑎𝑎𝑏𝑏,𝑎𝑎𝑎𝑎,𝑎𝑎1,𝑏𝑏1,𝑎𝑎2, 𝑏𝑏2,𝑎𝑎3,𝑏𝑏3,𝑎𝑎4, 𝑏𝑏4,𝑎𝑎5,𝑏𝑏5,𝑎𝑎6, 𝑏𝑏6],𝑞𝑞𝑖𝑖𝑖𝑖𝑑𝑑 = [𝑠𝑠1, 𝑠𝑠2, 𝑠𝑠3, 𝑠𝑠4, 𝑠𝑠5, 𝑠𝑠6]

The dynamic matrices can be found in 11.6.3.

The above formulation can also be used in the following configuration:

𝑞𝑞𝑑𝑑𝑑𝑑𝑑𝑑 = [𝑠𝑠1, 𝑠𝑠2, 𝑠𝑠3, 𝑠𝑠4, 𝑠𝑠5, 𝑠𝑠6,𝑎𝑎1,𝑏𝑏1,𝑎𝑎2,𝑏𝑏2,𝑎𝑎3, 𝑏𝑏3,𝑎𝑎4,𝑏𝑏4,𝑎𝑎5, 𝑏𝑏5,𝑎𝑎6,𝑏𝑏6],𝑞𝑞𝑖𝑖𝑖𝑖𝑑𝑑 = [𝑏𝑏𝑑𝑑,𝑏𝑏𝑑𝑑, 𝑎𝑎𝑑𝑑,𝑎𝑎𝑏𝑏,𝑎𝑎𝑏𝑏,𝑎𝑎𝑎𝑎]

After deriving the first set of equations in Maple and comparing the results with MapleSim, it is now

possble to eliminate all the universal joint angles from the constraint equation by breaking the mechanism

along the legs. In this case,

58 | P a g e

𝑞𝑞 = [𝑠𝑠1, 𝑠𝑠2, 𝑠𝑠3, 𝑠𝑠4, 𝑠𝑠5, 𝑠𝑠6,𝑏𝑏𝑑𝑑,𝑏𝑏𝑑𝑑, 𝑎𝑎𝑑𝑑,𝑎𝑎𝑏𝑏,𝑎𝑎𝑏𝑏,𝑎𝑎𝑎𝑎]

𝑞𝑞𝑑𝑑𝑑𝑑𝑑𝑑 = [𝑠𝑠1, 𝑠𝑠2, 𝑠𝑠3, 𝑠𝑠4, 𝑠𝑠5, 𝑠𝑠6],𝑞𝑞𝑖𝑖𝑖𝑖𝑑𝑑 = [𝑏𝑏𝑑𝑑,𝑏𝑏𝑑𝑑, 𝑎𝑎𝑑𝑑,𝑎𝑎𝑏𝑏,𝑎𝑎𝑏𝑏,𝑎𝑎𝑎𝑎]

OR

𝑞𝑞𝑑𝑑𝑑𝑑𝑑𝑑 = [𝑏𝑏𝑑𝑑,𝑏𝑏𝑑𝑑, 𝑎𝑎𝑑𝑑,𝑎𝑎𝑏𝑏,𝑎𝑎𝑏𝑏,𝑎𝑎𝑎𝑎], 𝑞𝑞𝑖𝑖𝑖𝑖𝑑𝑑 = [𝑠𝑠1, 𝑠𝑠2, 𝑠𝑠3, 𝑠𝑠4, 𝑠𝑠5, 𝑠𝑠6]

The resulting kinematic matrix is found in 11.6.4. In order to validate this manual formulation, we

compute the prismatic joint positions for the same elliptical trajectory using the 24-variable formulation and

the 12-variable formulation and compare the results.

Figure 41 : Kinematic Validation of 12 variable formulation of Hexapod

(a) Path of end effector with constant orientation (b) Difference in prismatic joint positions between two separate kinematic runs (c) Actual prismatic joint positions from 12 variable set (d) Position Constraints from 12 variable set

-2 -1.5 -1 -0.5 0 0.5 1 1.5 2

-2

-1

0

1

2-1.5

-1

-0.5

0

0.5

1

1.5

Y axis

Z ax

is

0 1 2 3 4 5 6 7-5

-4

-3

-2

-1

0

1

2

3

4

5x 10

-5

Time (s)

Erro

r in

Pris

mat

ic J

oint

Pos

ition

s

s1s2s3s4s5s6

0 1 2 3 4 5 6 7-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

Time (s)

Prismatic Joints Positions

123456

0 1 2 3 4 5 6 7-3

-2.5

-2

-1.5

-1

-0.5

0

0.5x 10

-10

Time (s)

Pos

ition

Con

stra

ints

Position Constraints

123456

59 | P a g e

The only matrix derived in this case was the constraint matrix since the angular velocities of the

connecting legs could not be calculated in terms of the end points available. This analysis could be

completed in future work.

6.2. Dynamic Analysis

As described in 3.2, the dynamics of the Hexapod is obtained using the Euler-Lagrange method of

EOM computation. The matrices are fairly complex and are included in full in the appendix.

6.3. Forward Dynamics comparison between Matlab/Simmechanics/Maplesim

The Maplesim model created for the Hexapod is as shown below:

Figure 42 - Hexapod Maplesim model

This model is then run with some initial conditions and a fixed time step solver. This will allow for

comparison with simulation results from other software simulations.

The Simmechanics model for the Hexapod is shown below.

60 | P a g e

Figure 43 : Overall model of Hexapod in Simmechanics

Figure 44 : Forward dynamics model of Hexapod in Simmechanics

The mechanism fails with the slightest of gravity perturbations and is not analyzed further in this work.

Only comparison between Maplesim and Matlab will be possible.

A Maplesim simulation was run for 6.28 seconds with a fixed time step of 0.001.

61 | P a g e

The same simulation was run using Matlab code using the same initial conditions and a comparison of

the results is shown below.

(a) (b)

(c) (d)

Figure 45 : Maplesim/Matlab forward dynamics comparison (a)Difference in prismatic positions calculated (b) Difference in prismatic velocities calculated (c) Actual prismatic positions calculated in Matlab (d) Position constraints (convergent)

It is apparent in Figure 45 (a) and (b) that the positions and velocities calculated by Maplesim and

Matlab match very closely (order of 10-6). Further, the position constraints to be maintained in the system

are also seen to converge steadily to zero. This sets the stage for the next step in the process, namely

control using inverse dynamics routine in Matlab and Forward dynamics routine from Maplesim or

Simmechanics.

0 1 2 3 4 5 6 7-1.5

-1

-0.5

0

0.5

1

1.5x 10

-6 Prismatic Joints Position Comparison

123456

0 1 2 3 4 5 6 7-2

-1.5

-1

-0.5

0

0.5

1

1.5

2x 10

-6 Prismatic Joints Velocity Comparison

s1dots2dots3dots4dots5dots6dot

0 1 2 3 4 5 6

-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5

Prismatic Joints Positions

123456

0 1 2 3 4 5 6 7-1.5

-1

-0.5

0

0.5

1

1.5x 10

-5 Position Constraints

123456789101112131415

62 | P a g e

6.4. Control Dynamics Simulation with Simmechanics

Due to problems with the forward dynamics model for the Hexapod in Simmechanics, this step was in

progress at the time this thesis was written and could be included in future work.

6.5. Further steps for Real-time implementation

To implement real-time control, the articulated model has to be brought reasonably close to the actual

model. Kinematically, the models are quite alike. Hence, one can find the dynamic properties of real

assemblies that are represented by single links in the AMBS and substitute them to get a model that is very

close to real-time. This same code can be then used for real-time implementation.

63 | P a g e

7. MapleSim for Education

MapleSim software was used earlier for automated symbolic kinematic and dynamic modeling and

simulation of the 3PRR and Hexapod models. In light of the ease with which the symbolic expressions for

the EOMs were generated, its productivity during a basic robotics course (covering the basics of concepts

like Lagrangian modeling and PM control) was explored. However, the use of such software in a robotics

course was limited because:

A. Currently the use of MapleSim needs “expert users” who can not only model, but also analyze the

results for their correctness. While tutorials are made available by the vendors of these tools, they

are targeted at a more experienced user (typically with a graduate level knowledge of AMBS).

These traditional tutorials may assume a certain level of both mathematical sophistication and

engineering experience from the user.

B. Novitiate robotics students may tend to have difficulty understanding both technical (theoretical)

concepts as well as their technological implementation. Moreover, it is crucial that student gleans a

greater insight into the problem and is better equipped to make engineering judgments from the

information obtained from the use of such software.

Our goal is to create a linkage between the Automated Symbolic Computing approach and the traditional analytical

approach so that the student can derive benefits from both – a better understanding of the problem as well as greater proficiency

in different methods available to solve it.

The implementation is as a series of tutorials with increasing complexity and decreasing support

starting from simple examples like Single Pendulum and Double Pendulum followed by intermediate

examples like the Fourbar and finally leading to the final projects for the course in terms of complex planar

mechanisms like the 3-PRR, 3-RPR etc.

A detailed description of the tutorials as well as their results is available in [19].

64 | P a g e

8. Future Scope

The real-time implementation of model-based control techniques on the Hexapod could be done by

following the same steps as for the 3PRR. Also, the constant position workspace of parallel platform

manipulators can be generated by modifying the Exact Workspace method with Geometric constraints. The

Exact Workspace method can be considered as a base for optimization algorithms because of its efficiency

and accuracy.

9. Conclusion

During the course of this thesis, a better understanding of the process of modeling, analysis, control

and refinement of articulated multi-body systems was obtained by taking a complicated model, simplifying it

with an articulated approximations and doing analyses on the articulated as well as complete model. We

have succeeded in making an articulated system from a complicated system. Kinematic and dynamic

analyses were done and both kinematic and dynamic control was successfully achieved for this model.

Also, the workspace analysis of parallel manipulators was studied and a new, more efficient method of

constant orientation workspace calculation was developed.

10. References

[1] Merlet, J. P., February 2006, Parallel Robots (Solid Mechanics and Its Applications), Springer. [2] Merlet, J. P., 2006, "Jacobian, Manipulability, Condition Number, and Accuracy of Parallel Robots," Journal of Mechanical Design, 128(1), pp. 199-206. [3] Liu, M. J., Li, C. X., and Li, C. N., 2000, "Dynamics Analysis of the Gough-Stewart Platform Manipulator," Ieee Transactions on Robotics and Automation, 16(1), pp. 94-98. [4] Mahmoodi, A., Menhaj, M. B., and Sabzehparvar, M., 2009, "An Efficient Method for Solution of Inverse Dynamics of Stewart Platform," Aircraft Engineering and Aerospace Technology, 81(5), pp. 398-406. [5] Vakil, M., Pendar, H., and Zohoor, H., 2008, "Closed-Form Dynamic Equations of the General Stewart Platform through the Newton-Euler Approach" And "A Newton-Euler Formulation for the Inverse Dynamics of the Stewart Platform Manipulator"," Mechanism and Machine Theory, 43(10), pp. 1349-1351. [6] Xi, F., and Sinatra, R., 2002, "Inverse Dynamics of Hexapods Using the Natural Orthogonal Complement Method," Journal of Manufacturing Systems, 21(2), pp. 73-82. [7] Sathiyanarayanan, M., 2008, "Analysis of Parallel Platform Architectures for Use in Masticatory Studies," M.S. Thesis, SUNY Buffalo, Buffalo. [8] Meng, J., Liu, G. F., and Li, Z. X., 2007, "A Geometric Theory for Analysis and Synthesis of Sub-6 Dof Parallel Manipulators," Ieee Transactions on Robotics, 23(4), pp. 625-649. [9] Staicu, S., 2009, "Inverse Dynamics of the 3-Prr Planar Parallel Robot," Robotics and Autonomous Systems, 57(5), pp. 556-563. [10] Staicu, S., 2009, "Power Requirement Comparison in the 3-Rpr Planar Parallel Robot Dynamics," Mechanism and Machine Theory, 44(5), pp. 1045-1057. [11] Saxena, V., Liu, D., Daniel, C. M., and Sutherland, J. W., 1997, "Simulation Study of the Workspace and Dexterity of a Stewart Platform Based Machine Tool," Proc. Proceedings of the 1997 ASME

65 | P a g e

International Mechanical Engineering Congress and Exposition, November 16, 1997 - November 21, 1997, Dallas, TX, USA, 61, pp. 617-623. [12] Bonev, I. A., and Gosselin, C. M., September 10-13, 2000, "Geometric Algorithms for the Computation of the Constant-Orientation Workspace and Singularity Surfaces of a Special 6-Rus Parallel Manipulator," Proc. ASME Design Engineernig Technical Conferences (DETC 2000), Baltimore, MD. [13] Jiang, Q., and Gosselin, C. M., 2009, "Evaluation and Representation of the Theoretical Orientation Workspace of the Gough-Stewart Platform," Journal of Mechanisms and Robotics, 1(2), pp. 9. [14] Merlet, J. P., 1999, "Determination of 6d Workspaces of Gough-Type Parallel Manipulator and Comparison between Different Geometries," International Journal of Robotics Research, 18(Compendex), pp. 902-916. [15] Yoshito Tanaka, I. Y., J. Ishii, M. Wakiyama, 1999, "Evaluation of Workspace in Parallel Machines." [16] Ryu, I. B. A. J., 1999, "Workspace Analysis of 6-Prrs Parallel Manipulators Based on the Vertex Space Concept," Proc. ASME Design Technical Conference, , Las Vegas, Nevada. [17] Pham, N., Joon-Ho, K., and Han-Sung, K., 2008, "Development of a New 6-Dof Parallel-Kinematic Motion Simulator (Iccas 2008)," Proc. Control, Automation and Systems, 2008. ICCAS 2008. International Conference on, pp. 2370-2373. [18] Pernkopf, F., and Husty, M. L., 2004, On Advances in Robot Kinematics, Sestri Levanta, Italy. [19] H. Shah, S. T., L-F. Lee, and V. Krovi, 2010, "Role of Automated Symbolic Generation of Equations of Motion in Mechanism and Robotics Education," Proc. ASME 2010 International Design Engineering Technical Conference, Montreal, Quebec, Canada.

11-1 | P a g e

11. Appendix

11.1. Parts and Specification

The significant parts in the Hexapod are listed below in the descending order of their characteristic

dimension.

Part name Nos.

Base platform 1

Hex table 1

Arm 6

Gimbal yoke upper 3

Gimbal yoke lower 3

Shaft support upper 3

Shaft support lower 3

Hex table BB 3

Arm BB 3

Gimbal cantilevers 30

11.2. 3-RPR Kinematics

11.2.1. 9 joint variables set

11.2.1.1. Positions and velocities of all points

Center of masses of first link, prismatic mass and second link

𝑏𝑏𝑖𝑖0 = 𝑏𝑏𝑏𝑏𝑖𝑖 +l𝑖𝑖12∙ cos(𝜃𝜃𝑖𝑖1)

𝑏𝑏𝑖𝑖0 = 𝑏𝑏𝑏𝑏𝑖𝑖 +l𝑖𝑖12∙ 𝑠𝑠𝑖𝑖𝑖𝑖(𝜃𝜃𝑖𝑖1)

𝑏𝑏𝑖𝑖1 = 𝑏𝑏𝑏𝑏𝑖𝑖 + (l𝑖𝑖1 + si) ∙ cos(𝜃𝜃𝑖𝑖1)

𝑏𝑏𝑖𝑖1 = 𝑏𝑏𝑏𝑏𝑖𝑖 + (l𝑖𝑖1 + si) ∙ 𝑠𝑠𝑖𝑖𝑖𝑖(𝜃𝜃𝑖𝑖1)

11-2 | P a g e

𝑏𝑏𝑖𝑖2 = 𝑏𝑏𝑏𝑏𝑖𝑖 + (l𝑖𝑖1 + si) ∙ cos(𝜃𝜃𝑖𝑖1) +𝑙𝑙𝑖𝑖22∙ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)

𝑏𝑏𝑖𝑖2 = 𝑏𝑏𝑏𝑏𝑖𝑖 + (l𝑖𝑖1 + si) ∙ sin(𝜃𝜃𝑖𝑖1) +𝑙𝑙𝑖𝑖22∙ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)

Center of platform:

𝑏𝑏𝑖𝑖3 = 𝑏𝑏𝑏𝑏𝑖𝑖 + (l𝑖𝑖1 + si) ∙ cos(𝜃𝜃𝑖𝑖1) + 𝑙𝑙𝑖𝑖2 ∙ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)

𝑏𝑏𝑖𝑖3 = 𝑏𝑏𝑏𝑏𝑖𝑖 + (l𝑖𝑖1 + si) ∙ sin(𝜃𝜃𝑖𝑖1) + 𝑙𝑙𝑖𝑖2 ∙ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)

Now, differentiating the position level equations, we get

𝑏𝑏𝑖𝑖0 = −l𝑖𝑖12∙ 𝑠𝑠𝑖𝑖𝑖𝑖(𝜃𝜃𝑖𝑖1)

𝑏𝑏𝑖𝑖0 =l𝑖𝑖12∙ cos(𝜃𝜃𝑖𝑖1)

𝑏𝑏𝑖𝑖1 = −(l𝑖𝑖1 + si) ∙ 𝑠𝑠𝑖𝑖𝑖𝑖(𝜃𝜃𝑖𝑖1) + si ∙ cos(𝜃𝜃𝑖𝑖1)

𝑏𝑏𝑖𝑖1 = (l𝑖𝑖1 + si) ∙ cos(𝜃𝜃𝑖𝑖1) + si ∙ sin(𝜃𝜃𝑖𝑖1)

𝑏𝑏𝑖𝑖2 = −(l𝑖𝑖1 + si) ∙ 𝑠𝑠𝑖𝑖𝑖𝑖(𝜃𝜃𝑖𝑖1) + si ∙ cos(𝜃𝜃𝑖𝑖1) −𝑙𝑙𝑖𝑖22∙ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2) ∙ (𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 )

𝑏𝑏𝑖𝑖2 = (l𝑖𝑖1 + si) ∙ cos(𝜃𝜃𝑖𝑖1) + si ∙ sin(𝜃𝜃𝑖𝑖1) +𝑙𝑙𝑖𝑖22∙ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2) ∙ (𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 )

𝑏𝑏𝑖𝑖3 = −(l𝑖𝑖1 + si) ∙ 𝑠𝑠𝑖𝑖𝑖𝑖(𝜃𝜃𝑖𝑖1) + si ∙ cos(𝜃𝜃𝑖𝑖1) − 𝑙𝑙𝑖𝑖2 ∙ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2) ∙ (𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 )

𝑏𝑏𝑖𝑖3 = (l𝑖𝑖1 + si) ∙ cos(𝜃𝜃𝑖𝑖1) + si ∙ sin(𝜃𝜃𝑖𝑖1) + 𝑙𝑙𝑖𝑖2 ∙ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2) ∙ (𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 )

The rest of the analysis is covered in the thesis.

11.2.2. Set of 6 joint variables and 3 task variables

xC=[-1,0,-1,0,-1,0;

0,-1,0,-1,0,-1;

-sin(φ)*l/sq3,cos(φ)*l/sq3,1/2*(sin(φ)*l+l*cos(φ)*sq3)/sq3,1/2*(-cos(φ)*l+l*sin(φ)*sq3)/sq3,1/2*(sin(φ)*l-

l*cos(φ)*sq3)/sq3,1/2*(-cos(φ)*l-l*sin(φ)*sq3)/sq3;

cos(th11),sin(th11),0,0,0,0;

-sin(th11)*s1-l11*sin(th11),cos(th11)*s1+l11*cos(th11),0,0,0,0;

0,0,cos(th21),sin(th21),0,0;

0,0,-l21*sin(th21)-sin(th21)*s2,l21*cos(th21)+cos(th21)*s2,0,0;

0,0,0,0,cos(th31),sin(th31);

0,0,0,0,-l31*sin(th31)-sin(th31)*s3,cos(th31)*s3+l31*cos(th31)]’;

11-3 | P a g e

11.3. Gauss Divergence Theorem Calculations

11.3.1. Circular Arc

A circular arc is represented by its center (xc,yc), its radius rc and the bounding angles θ1 and θ2.

Hence, at any given angle θ,

𝑖𝑖� = cos(𝜃𝜃) 𝑖𝑖 + sin(𝜃𝜃) 𝑗𝑗, 𝑏𝑏 = 𝑏𝑏𝑐𝑐 + 𝑝𝑝𝑐𝑐 ∙ cos(𝜃𝜃) , 𝑏𝑏 = 𝑏𝑏𝑐𝑐 + 𝑝𝑝𝑐𝑐 ∙ sin(𝜃𝜃) , 𝑑𝑑𝑠𝑠 = 𝑝𝑝𝑐𝑐 ∙ 𝑑𝑑𝜃𝜃

𝐹𝐹 ∙ 𝑖𝑖� =(𝑏𝑏𝑐𝑐 + 𝑝𝑝𝑐𝑐 ∙ cos(𝜃𝜃)) ∙ cos(𝜃𝜃) + (𝑏𝑏𝑐𝑐 + 𝑝𝑝𝑐𝑐 ∙ sin(𝜃𝜃)) ∙ sin(𝜃𝜃)

2=𝑏𝑏𝑐𝑐 ∙ cos(𝜃𝜃) + 𝑏𝑏𝑐𝑐 ∙ sin(𝜃𝜃) + 𝑝𝑝𝑐𝑐

2

� 𝑭𝑭.𝒏𝒏�𝑑𝑑𝑠𝑠𝑑𝑑𝑆𝑆

=12� (𝑏𝑏𝑐𝑐 ∙ cos(𝜃𝜃) + 𝑏𝑏𝑐𝑐 ∙ sin(𝜃𝜃) + 𝑝𝑝𝑐𝑐𝜃𝜃2

𝜃𝜃1

) ∙ 𝑝𝑝𝑐𝑐 ∙ 𝑑𝑑𝜃𝜃

=12 �𝑝𝑝𝑐𝑐 ∙ 𝑏𝑏𝑐𝑐 ∙ (sin(𝜃𝜃2)− sin(𝜃𝜃1))− 𝑝𝑝𝑐𝑐 ∙ 𝑏𝑏𝑐𝑐 ∙ (cos(𝜃𝜃2) − cos(𝜃𝜃1)) + 𝑝𝑝𝑐𝑐2 ∙ (𝜃𝜃2 − 𝜃𝜃1)�

Equation 11-1 : Area under circular arc using Gauss divergence theorem

11.3.2. Line

For a line going from point A (x1,y1) to point B (x2,y2).

𝑑𝑑 = �(𝑏𝑏2 − 𝑏𝑏1)2 + (𝑏𝑏2 − 𝑏𝑏1)2, 𝑖𝑖� = �𝑏𝑏1 − 𝑏𝑏2

𝑑𝑑� 𝑖𝑖 + (

y2 − y1d

)𝑗𝑗

𝑏𝑏 = 𝑏𝑏1 + 𝑏𝑏1 ∙ (𝑏𝑏2 − 𝑏𝑏1), 𝑏𝑏 = 𝑏𝑏1 + 𝑏𝑏1 ∙ (𝑏𝑏2 − 𝑏𝑏1)

𝐹𝐹 ∙ 𝑖𝑖� =�𝑏𝑏1 + 𝑏𝑏1 ∙ (𝑏𝑏1 − 𝑏𝑏2)� ∙ (𝑏𝑏1 − 𝑏𝑏2) + �𝑏𝑏1 + 𝑏𝑏1 ∙ (𝑏𝑏2 − 𝑏𝑏1)� ∙ (𝑏𝑏2 − 𝑏𝑏1)

2 ∙ 𝑑𝑑 =𝑏𝑏1

2 − 𝑏𝑏1 ∙ 𝑏𝑏2 + 𝑏𝑏1 ∙ 𝑏𝑏2 − 𝑏𝑏12 + 𝑏𝑏1 ∙ 𝑑𝑑2

2 ∙ 𝑑𝑑

� 𝑭𝑭.𝒏𝒏�𝑑𝑑𝑠𝑠𝑑𝑑𝑆𝑆

=1

2 ∙ 𝑑𝑑� �𝑏𝑏1

2 − 𝑏𝑏1 ∙ 𝑏𝑏2 + 𝑏𝑏1 ∙ 𝑏𝑏2 − 𝑏𝑏12 + 𝑏𝑏1 ∙ 𝑑𝑑2� ∙ 𝑑𝑑𝑏𝑏1

𝑏𝑏1𝑏𝑏

𝑏𝑏1𝑎𝑎

=�𝑏𝑏1

2 − 𝑏𝑏1 ∙ 𝑏𝑏2 + 𝑏𝑏1 ∙ 𝑏𝑏2 − 𝑏𝑏12� ∙ (𝑏𝑏1𝑏𝑏 − 𝑏𝑏1𝑎𝑎)

2 ∙ 𝑑𝑑+𝑑𝑑 ∙ (𝑏𝑏1𝑏𝑏2 − 𝑏𝑏1𝑎𝑎2)

4

Equation 11-2 : Area under line using Gauss divergence theorem

11-4 | P a g e

11.4. Maple Code for Hexapod Dynamic Analysis

This is a discussion of the code used to generate the dynamic EOMs for the Hexapod in Maple.

The next step is to declare the transformation matrices from the ground frame to the reference frames

Ai (as shown in Figure 16). Also, T7 refers to the transformation matrix relating only the rotations of the end-

effector reference frame.

The next step is the calculation of the positions of the centers of mass for the six legs. It is stored in

X2i. X2ai refer to the position of the end of the six legs connected to the platform.

The next step is the calculation of the position of the center and the end-points of the platform. The

end-point is also calculated by using Equation 4-4(1) on the second leg. This is done to match the

Maplesim formulation of Hexapod EOM.

11-5 | P a g e

The next step is to calculate the linear and angular velocities of the six legs and the platform. The

linear velocities are computed easily by differentiation and the angular velocities are computed by using the

twist formulation.

The next step is the computation of the Lagrangian of the system, which is the difference of the kinetic

and potential energies of the system. To do this, the mass, position, Moment of inertia about principal axes

11-6 | P a g e

and angular velocity of all the masses are stored in a matrix, which is then run through a loop in the

function ‘LagCalc’ to calculate the Lagrangian.

The next step is the declaration of the variable space, its derivatives and double-derivatives.

The next step is to run through all the variables and store the resulting Euler-Lagrange equations in a

matrix. This is done using either the “EulerLagrange” command in Maple or using a user-written function as

shown here.

11-7 | P a g e

The next step is the extraction of the mass matrix and force vector from the Euler-Lagrange equations.

The mass matrix is extracted by isolating the terms relating to the double-derivatives of the states from

each equation. The force vector is computed as the difference of the total equation and the product of the

mass matrix and double-differential of all the states.

The last step is the creation of the position constraints in the system. This is done by equating the

coordinates of the end-points of the platform and converting them to the reference frames at those points.

This is done to get to the same result as Maplesim.

11.5. 3PRR Dynamic Equations

11.5.1. Substitutions

cos(th11)=c11; cos(th12)=c12; cos(th21)=c21; cos(th22)=c22; cos(th31)=c31; cos(th32)=c32;

sin(th11)=s11; sin(th12)=s12; sin(th21)=s21; sin(th22)=s22; sin(th31)=s31; sin(th32)=s32;

cos(4/3*pi)=c43pi; sin(4/3*pi)=s43pi;

11-8 | P a g e

11.5.2. 7 variable set

11.5.2.1. Force Vector

vF=[m13*l12*th12d*th11d*c11*c12-

m13*l12*th12d*th11d*s11*s12+1/2*m13*l12*th12d^2*c11*c12+m13*l12*th12d^2*s11*s12*c43pi+1/2*m12*l11*th11d^2*c11+m13*l11*th11d^

2*c11+1/2*m13*l12*th11d^2*c11*c12+m13*l12*th11d^2*s11*s12*c43pi;

1/2*m13*l11*l12*th12d^2*s12+m13*l11*th11d*l12*s12*th12d;

m13*l11*th11d^2*l12*s12*c43pi+m33*l31*th31d^2*l32*s12*c43pi*c43pi+m33*l31*th31d^2*l32*c12*s43pi*c43pi;

1/2*m22*l21*th21d^2*c21+m23*l21*th21d^2*c21+1/2*th21d^2*m23*l22*c21*c22+th21d^2*m23*l22*s21*s22*c43pi;

0;

m33*l32*th12d*th31d*c31*c12*c43pi-m33*l32*th12d*th31d*c31*s12*s43pi-m33*l32*th12d*th31d*s31*s12*c43pi-

m33*l32*th12d*th31d*s31*c12*s43pi+1/2*m33*l32*th12d^2*c31*c12*c43pi+m33*l32*th12d^2*c31*s12*s43pi*c43pi+m33*l32*th12d^2*s31*

s12*c43pi*c43pi+m33*l32*th12d^2*s31*c12*s43pi*c43pi+1/2*m32*l31*th31d^2*c31+m33*l31*th31d^2*c31+1/2*m33*l32*th31d^2*c31*c12*

c43pi+m33*l32*th31d^2*c31*s12*s43pi*c43pi+m33*l32*th31d^2*s31*s12*c43pi*c43pi+m33*l32*th31d^2*s31*c12*s43pi*c43pi;

m33*l31*l32*th12d*s12*c43pi*th31d+1/2*m33*l31*l32*th12d^2*s12*c43pi+m33*l31*l32*th12d*c12*s43pi*th31d+1/2*m33*l31*l32*th1

2d^2*c12*s43pi];

11.5.2.2. Mass Matrix

xM=[-m11-m13-m12, 1/2*m12*l11*s11+m13*l11*s11+1/2*m13*l12*s11*c12+1/2*m13*l12*c11*s12,

1/2*m13*l12*s11*c12+1/2*m13*l12*c11*s12, 0, 0, 0, 0;

1/2*m12*l11*s11+m13*l11*s11+1/2*m13*l12*s11*c12+1/2*m13*l12*c11*s12, -1/4*m12*l11^2-m13*l11^2-1/4*m13*l12^2-J12-J13-

m13*l11*l12*c12, -1/2*m13*l11*l12*c12-J13-1/4*m13*l12^2, 0, 0, 0, 0;

1/2*m13*l12*s11*c12+1/2*m13*l12*c11*s12, -1/2*m13*l11*l12*c12-J13-1/4*m13*l12^2, -1/4*m13*l12^2-J13-1/4*m33*l32^2-J33, 0,

0, 1/2*m33*l32*s31*c12*c43pi-1/2*m33*l32*s31*s12*s43pi+1/2*m33*l32*c31*s12*c43pi+1/2*m33*l32*c31*c12*s43pi, -1/4*m33*l32^2-

J33-1/2*m33*l31*l32*c12*c43pi+1/2*m33*l31*l32*s12*s43pi;

0, 0, 0, -m21-m22-m23, 1/2*m22*l21*s21+m23*l21*s21+1/2*m23*l22*s21*c22+1/2*m23*l22*c21*s22, 0, 0;

0, 0, 0, 1/2*m22*l21*s21+m23*l21*s21+1/2*m23*l22*s21*c22+1/2*m23*l22*c21*s22, -1/4*m22*l21^2-m23*l21^2-1/4*m23*l22^2-

m23*l21*l22*c22-J22-J23, 0, 0;

0, 0, 1/2*m33*l32*s31*c12*c43pi-1/2*m33*l32*s31*s12*s43pi+1/2*m33*l32*c31*s12*c43pi+1/2*m33*l32*c31*c12*s43pi, 0, 0, -

m31-m32-m33, 1/2*m33*l32*s31*c12*c43pi-

1/2*m33*l32*s31*s12*s43pi+1/2*m33*l32*c31*s12*c43pi+1/2*m33*l32*c31*c12*s43pi+1/2*m32*l31*s31+m33*l31*s31;

0, 0, -1/4*m33*l32^2-J33-1/2*m33*l31*l32*c12*c43pi+1/2*m33*l31*l32*s12*s43pi, 0, 0, 1/2*m33*l32*s31*c12*c43pi-

1/2*m33*l32*s31*s12*s43pi+1/2*m33*l32*c31*s12*c43pi+1/2*m33*l32*c31*c12*s43pi+1/2*m32*l31*s31+m33*l31*s31, -1/4*m33*l32^2-

J33-m33*l31*l32*c12*c43pi+m33*l31*l32*s12*s43pi-1/4*m32*l31^2-m33*l31^2-J32];

11.5.2.3. Constraint Matrix

xC=[cal1, sal1, cal1, sal1;

-l11*sal1*c11-l11*cal1*s11-l12*sal1*c11*c12+l12*sal1*s11*s12-l12*cal1*s11*c12-l12*cal1*c11*s12, l11*cal1*c11-

l11*sal1*s11+l12*cal1*c11*c12-l12*cal1*s11*s12-l12*sal1*s11*c12-l12*sal1*c11*s12, -l11*sal1*c11-l11*cal1*s11-

l12*sal1*c11*c12+l12*sal1*s11*s12-l12*cal1*s11*c12-l12*cal1*c11*s12, l11*cal1*c11-l11*sal1*s11+l12*cal1*c11*c12-

l12*cal1*s11*s12-l12*sal1*s11*c12-l12*sal1*c11*s12;

-l12*sal1*c11*c12+l12*sal1*s11*s12-l12*cal1*s11*c12-l12*cal1*c11*s12, l12*cal1*c11*c12-l12*cal1*s11*s12-l12*sal1*s11*c12-

l12*sal1*c11*s12, -l12*sal1*c11*c12+l12*sal1*s11*s12-l12*cal1*s11*c12-l12*cal1*c11*s12+l32*sal3*c31*c12*c43pi-

l32*sal3*c31*s12*s43pi-l32*sal3*s31*s12*c43pi-l32*sal3*s31*c12*s43pi+l32*cal3*s31*c12*c43pi-

l32*cal3*s31*s12*s43pi+l32*cal3*c31*s12*c43pi+l32*cal3*c31*c12*s43pi, l12*cal1*c11*c12-l12*cal1*s11*s12-l12*sal1*s11*c12-

l12*sal1*c11*s12-

l32*cal3*c31*c12*c43pi+l32*cal3*c31*s12*s43pi+l32*cal3*s31*s12*c43pi+l32*cal3*s31*c12*s43pi+l32*sal3*s31*c12*c43pi-

l32*sal3*s31*s12*s43pi+l32*sal3*c31*s12*c43pi+l32*sal3*c31*c12*s43pi;

-cal2, -sal2, 0, 0;

l21*sal2*c21+l21*cal2*s21+l22*sal2*c21*c22-l22*sal2*s21*s22+l22*cal2*s21*c22+l22*cal2*c21*s22, -21*cal2*c21+l21*sal2*s21-

l22*cal2*c21*c22+l22*cal2*s21*s22+l22*sal2*s21*c22+l22*sal2*c21*s22, 0, 0;

0, 0, -cal3, -sal3;

0, 0, l31*sal3*c31+l31*cal3*s31+l32*sal3*c31*c12*c43pi-l32*sal3*c31*s12*s43pi-l32*sal3*s31*s12*c43pi-

l32*sal3*s31*c12*s43pi+l32*cal3*s31*c12*c43pi-l32*cal3*s31*s12*s43pi+l32*cal3*c31*s12*c43pi+l32*cal3*c31*c12*s43pi, -

l31*cal3*c31+l31*sal3*s31-

11-9 | P a g e

l32*cal3*c31*c12*c43pi+l32*cal3*c31*s12*s43pi+l32*cal3*s31*s12*c43pi+l32*cal3*s31*c12*s43pi+l32*sal3*s31*c12*c43pi-

l32*sal3*s31*s12*s43pi+l32*sal3*c31*s12*c43pi+l32*sal3*c31*c12*s43pi]’;

11.6. Hexapod Dynamic Equations

11.6.1. Substitutions

sal1=sin(al(1)); sal2=sin(al(2)); sal3=sin(al(3));

cal1=cos(al(1)); cal2=cos(al(2)); cal3=cos(al(3));

sax=sin(ax); say=sin(ay); saz=sin(az);

cax=cos(ax); cay=cos(ay); caz=cos(az);

ca1=cos(a1); ca2=cos(a2); ca3=cos(a3); ca4=cos(a4); ca5=cos(a5); ca6=cos(a6);

sa1=sin(a1); sa2=sin(a2); sa3=sin(a3); sa4=sin(a4); sa5=sin(a5); sa6=sin(a6);

cb1=cos(b1); cb2=cos(b2); cb3=cos(b3); cb4=cos(b4); cb5=cos(b5); cb6=cos(b6);

sb1=sin(b1); sb2=sin(b2); sb3=sin(b3); sb4=sin(b4); sb5=sin(b5); sb6=sin(b6);



vF=[1/2*m1*l1*bd1^2*sb1;

2/3*M*l*sq3*axd*sal1*sax*saz*azd+2/3*M*ayd*l*sq3*cal1*say*caz*azd-

2/3*M*l*sq3*axd*sal1*saz*cax*cay*ayd+1/3*M*l*sq3*axd^2*sax*say*saz*sal1-2/3*M*l*sq3*axd*sal1*cax*say*caz*azd-

1/3*M*l*sq3*axd^2*cax*caz*sal1-

1/3*M*azd^2*l*sq3*cax*caz*sal1+1/3*M*ayd^2*l*sq3*cay*saz*cal1+1/3*M*azd^2*l*sq3*cay*saz*cal1+1/3*M*ayd^2*l*sq3*sax*say*saz*s

al1-2/3*M*ayd*l*sq3*sal1*caz*sax*cay*azd+1/2*m2*l2*bd2^2*sb2+M*l2*bd2^2*sb2+1/3*M*azd^2*l*sq3*sax*say*saz*sal1;

1/2*m3*l3*bd3^2*sb3;

1/2*m4*l4*bd4^2*sb4;

1/2*m5*l5*bd5^2*sb5;

1/2*m6*l6*bd6^2*sb6;

-JX*ayd*cay*azd+JY*ayd*cay*azd+JZ*azd*cay*ayd+2/3*M*l^2*axd*ayd*say*cay-

1/3*M*l^2*ayd^2*say*saz*caz+1/3*M*g*l*sq3*cax*caz+2/3*M*l^2*ayd*cay*caz^2*azd-2*JY*axd*say*cay*ayd+2*JZ*axd*ayd*say*cay-

JX*ayd^2*say*saz*caz+JY*ayd^2*say*saz*caz+2*JX*ayd*cay*caz^2*azd-2*JY*ayd*cay*caz^2*azd-1/3*M*g*l*sq3*sax*say*saz-

2/3*M*l^2*axd*say*cay*caz^2*ayd-2/3*M*l^2*axd*cay^2*saz*caz*azd-1/3*M*l2*bd2^2*l*sq3*sal1*sb2*sax*caz-

2/3*M*l2*ad2*l*sq3*sb2*saz*cax*sa2*say*cal1*bd2-

2/3*M*l2*ad2*l*sq3*sb2*sax*caz*cal1*sa2*bd2+1/3*M*l2*ad2^2*l*sq3*cb2*saz*cax*ca2*say*cal1+1/3*M*l2*ad2^2*l*sq3*cb2*sax*caz*c

al1*ca2+1/3*M*l2*bd2^2*l*sq3*cb2*sax*caz*cal1*ca2+2/3*M*l2*bd2*l*sq3*ca2*sb2*sax*say*saz*ad2-

2/3*M*l2*bd2*l*sq3*ca2*sb2*cax*caz*ad2+1/3*M*l2*ad2^2*l*sq3*sa2*cb2*sax*say*saz+1/3*M*l2*bd2^2*l*sq3*sa2*cb2*sax*say*saz-

1/3*M*l2*bd2^2*l*sq3*sal1*sb2*cax*say*saz-1/3*M*l2*ad2^2*l*sq3*sa2*cb2*cax*caz-

1/3*M*l2*bd2^2*l*sq3*sa2*cb2*cax*caz+1/3*M*l2*bd2^2*l*sq3*cb2*saz*cax*ca2*say*cal1-

2*JX*axd*say*cay*caz^2*ayd+2*JY*axd*say*cay*caz^2*ayd-2*JX*axd*cay^2*saz*caz*azd+2*JY*axd*cay^2*saz*caz*azd;

JY*axd^2*say*cay-JZ*axd^2*say*cay-JX*axd*cay*azd+JY*axd*cay*azd-

JZ*azd*cay*axd+2/3*M*ayd*l^2*azd*saz*caz+2/3*M*l^2*axd*cay*caz^2*azd+1/3*M*l^2*axd^2*say*cay*caz^2-2/3*M*l^2*axd*azd*cay-

1/3*M*l^2*axd^2*say*cay+2*JX*ayd*azd*saz*caz-2*JY*ayd*azd*saz*caz+JX*axd^2*say*cay*caz^2-

JY*axd^2*say*cay*caz^2+2*JX*axd*cay*caz^2*azd-

2*JY*axd*cay*caz^2*azd+1/3*M*g*l*sq3*saz*cax*cay+1/3*M*l2*bd2^2*l*sq3*cal1*sb2*say*saz-

1/3*M*l2*ad2^2*l*sq3*sa2*cb2*saz*cax*cay-1/3*M*l2*bd2^2*l*sq3*sa2*cb2*saz*cax*cay-

2/3*M*l2*ad2*l*sq3*sb2*say*saz*sal1*sa2*bd2-1/3*M*l2*bd2^2*l*sq3*sal1*sb2*saz*sax*cay-

2/3*M*l2*ad2*l*sq3*sb2*saz*sax*cay*cal1*sa2*bd2+1/3*M*l2*bd2^2*l*sq3*cb2*saz*sax*ca2*cay*cal1+1/3*M*l2*ad2^2*l*sq3*cb2*saz*s

ax*ca2*cay*cal1+1/3*M*l2*bd2^2*l*sq3*cb2*say*saz*sal1*ca2+1/3*M*l2*ad2^2*l*sq3*cb2*say*saz*sal1*ca2-

2/3*M*l2*bd2*l*sq3*ca2*sb2*saz*cax*cay*ad2;

JX*axd*ayd*cay-JY*axd*ayd*cay+JZ*cay*ayd*axd-JX*ayd^2*saz*caz+JY*ayd^2*saz*caz-1/3*M*g*l*sq3*sax*saz-

2/3*M*l^2*axd*ayd*cay*caz^2+1/3*M*l^2*axd^2*cay^2*saz*caz+2/3*M*l^2*axd*cay*ayd-

1/3*M*ayd^2*l^2*saz*caz+JX*axd^2*cay^2*saz*caz-JY*axd^2*cay^2*saz*caz-

2*JX*axd*ayd*cay*caz^2+2*JY*axd*ayd*cay*caz^2+1/3*M*g*l*sq3*cax*say*caz-1/3*M*l2*bd2^2*l*sq3*sal1*sb2*sax*say*caz-

1/3*M*l2*bd2^2*l*sq3*cal1*sb2*cay*caz+2/3*M*l2*ad2*l*sq3*sb2*cay*caz*sal1*sa2*bd2-1/3*M*l2*ad2^2*l*sq3*sa2*cb2*cax*say*caz-

1/3*M*l2*bd2^2*l*sq3*sa2*cb2*cax*say*caz+1/3*M*l2*bd2^2*l*sq3*sa2*cb2*sax*saz+1/3*M*l2*ad2^2*l*sq3*sa2*cb2*sax*saz-

11-10 | P a g e

1/3*M*l2*bd2^2*l*sq3*sal1*sb2*cax*saz-2/3*M*l2*ad2*l*sq3*sb2*sax*say*caz*cal1*sa2*bd2-

2/3*M*l2*ad2*l*sq3*sb2*cax*saz*cal1*sa2*bd2-1/3*M*l2*bd2^2*l*sq3*cb2*cay*caz*sal1*ca2-

1/3*M*l2*ad2^2*l*sq3*cb2*cay*caz*sal1*ca2+1/3*M*l2*bd2^2*l*sq3*cb2*cax*saz*cal1*ca2+1/3*M*l2*ad2^2*l*sq3*cb2*cax*saz*cal1*ca

2+1/3*M*l2*bd2^2*l*sq3*cb2*caz*sax*ca2*say*cal1+1/3*M*l2*ad2^2*l*sq3*cb2*caz*sax*ca2*say*cal1-

2/3*M*l2*bd2*l*sq3*ca2*sb2*cax*say*caz*ad2+2/3*M*l2*bd2*l*sq3*ca2*sb2*sax*saz*ad2;

-1/2*m1*l1^2*ad1*bd1*sb1*cb1-2*ad1*Jx*bd1*sb1*cb1+2*ad1*Jy*bd1*sb1*cb1+1/2*m1*g*l1*ca1*cb1;

ad1^2*Jx*sb1*cb1-ad1^2*Jy*sb1*cb1+1/4*m1*l1^2*ad1^2*sb1*cb1-1/2*m1*g*l1*sa1*sb1;

-2*ad2*Jx*bd2*sb2*cb2+2*ad2*Jy*bd2*sb2*cb2-1/2*m2*l2^2*ad2*bd2*sb2*cb2-

2*M*l2^2*ad2*bd2*sb2*cb2+1/2*m2*g*l2*ca2*cb2+M*g*l2*ca2*cb2+2/3*M*l2*ayd*l*sq3*cb2*say*caz*sal1*sa2*azd+1/3*M*l2*ayd^2*l*sq3

*cb2*cay*saz*sal1*sa2+1/3*M*l2*azd^2*l*sq3*cb2*cay*saz*sal1*sa2+2/3*M*l2*l*sq3*axd*cb2*caz*cax*sa2*say*cal1*azd+1/3*M*l2*azd

^2*l*sq3*cb2*cax*caz*cal1*sa2+1/3*M*l2*l*sq3*axd^2*cb2*cax*caz*cal1*sa2+2/3*M*l2*ayd*l*sq3*cb2*caz*sax*cay*cal1*sa2*azd+2/3*

M*l2*l*sq3*axd*cb2*saz*cax*sa2*cay*cal1*ayd-2/3*M*l2*l*sq3*axd*cb2*sax*saz*cal1*sa2*azd-

1/3*M*l2*ayd^2*l*sq3*cb2*sax*say*saz*cal1*sa2-1/3*M*l2*azd^2*l*sq3*cb2*sax*say*saz*cal1*sa2-

1/3*M*l2*l*sq3*axd^2*cb2*sax*say*saz*cal1*sa2-1/3*M*l2*azd^2*l*sq3*ca2*cb2*cax*say*saz-

1/3*M*l2*l*sq3*axd^2*ca2*cb2*cax*say*saz-1/3*M*l2*ayd^2*l*sq3*ca2*cb2*cax*say*saz-2/3*M*l2*l*sq3*axd*ca2*cb2*cax*saz*azd-

2/3*M*l2*ayd*l*sq3*ca2*cb2*saz*sax*cay*axd-1/3*M*l2*azd^2*l*sq3*ca2*cb2*sax*caz-1/3*M*l2*l*sq3*axd^2*ca2*cb2*sax*caz-

2/3*M*l2*azd*l*sq3*ca2*cb2*sax*say*caz*axd+2/3*M*l2*ayd*l*sq3*ca2*cb2*caz*cax*cay*azd;

2/3*M*l2*azd*l*sq3*sal1*cb2*cax*say*caz*axd+1/3*M*l2*azd^2*l*sq3*cax*caz*sal1*cb2+1/3*M*l2*l*sq3*axd^2*cax*caz*sal1*cb2+2

/3*M*l2*azd*l*sq3*sal1*cb2*caz*sax*cay*ayd-1/3*M*l2*ayd^2*l*sq3*cay*saz*cal1*cb2-1/3*M*l2*azd^2*l*sq3*cay*saz*cal1*cb2-

2/3*M*l2*azd*l*sq3*cal1*cb2*say*caz*ayd+1/3*M*l2*l*sq3*axd^2*cax*say*saz*sa2*sb2+1/3*M*l2*ayd^2*l*sq3*cax*say*saz*sa2*sb2+2/

3*M*l2*l*sq3*axd*sa2*sb2*cax*saz*azd+1/3*M*l2*azd^2*l*sq3*cax*say*saz*sa2*sb2+2/3*M*l2*azd*l*sq3*sa2*sb2*sax*say*caz*axd+1/3

*M*l2*l*sq3*axd^2*sax*caz*sa2*sb2+1/3*M*l2*azd^2*l*sq3*sax*caz*sa2*sb2-

2/3*M*l2*ayd*l*sq3*sa2*sb2*caz*cax*cay*azd+2/3*M*l2*ayd*l*sq3*sa2*sb2*saz*sax*cay*axd+2/3*M*l2*l*sq3*axd*sal1*cb2*saz*cax*ca

y*ayd-2/3*M*l2*l*sq3*axd*sal1*cb2*sax*saz*azd-1/3*M*l2*ayd^2*l*sq3*sax*say*saz*sal1*cb2-

1/3*M*l2*azd^2*l*sq3*sax*say*saz*sal1*cb2-

1/3*M*l2*l*sq3*axd^2*sax*say*saz*sal1*cb2+2/3*M*l2*ayd*l*sq3*sb2*say*caz*sal1*ca2*azd+2/3*M*l2*l*sq3*axd*sb2*saz*cax*ca2*cay

*cal1*ayd-2/3*M*l2*l*sq3*axd*sb2*sax*saz*cal1*ca2*azd-1/3*M*l2*ayd^2*l*sq3*sax*say*saz*cal1*ca2*sb2-

1/3*M*l2*azd^2*l*sq3*sax*say*saz*cal1*ca2*sb2-

1/3*M*l2*l*sq3*axd^2*sax*say*saz*cal1*ca2*sb2+2/3*M*l2*l*sq3*axd*sb2*caz*cax*ca2*say*cal1*azd+1/4*m2*l2^2*ad2^2*sb2*cb2-

1/2*m2*g*l2*sa2*sb2+M*l2^2*ad2^2*sb2*cb2-M*g*l2*sa2*sb2+ad2^2*Jx*sb2*cb2-

ad2^2*Jy*sb2*cb2+1/3*M*l2*l*sq3*axd^2*cax*caz*cal1*ca2*sb2+1/3*M*l2*azd^2*l*sq3*cax*caz*cal1*ca2*sb2+2/3*M*l2*ayd*l*sq3*sb2*

caz*sax*ca2*cay*cal1*azd+1/3*M*l2*ayd^2*l*sq3*cay*saz*sal1*ca2*sb2+1/3*M*l2*azd^2*l*sq3*cay*saz*sal1*ca2*sb2;


1/4*m3*l3^2*ad3^2*sb3*cb3+ad3^2*Jx*sb3*cb3-ad3^2*Jy*sb3*cb3-1/2*m3*g*l3*sa3*sb3;






-1/2*m6*g*l6*sa6*sb6+ad6^2*Jx*sb6*cb6-ad6^2*Jy*sb6*cb6+1/4*m6*l6^2*ad6^2*sb6*cb6];


xM=[m1, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/2*m1*l1*cb1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

0, m2+M, 0, 0, 0, 0, -1/3*M*l*sq3*sal1*sax*caz-1/3*M*l*sq3*sal1*cax*say*saz, 1/3*M*l*sq3*cal1*say*saz-

1/3*M*l*sq3*sal1*saz*sax*cay, -1/3*M*l*sq3*cay*caz*cal1-1/3*M*l*sq3*cax*saz*sal1-1/3*M*l*sq3*sax*say*caz*sal1, 0, 0, 0, -

1/2*m2*l2*cb2-M*l2*cb2, 0, 0, 0, 0, 0, 0, 0, 0;

0, 0, m3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/2*m3*l3*cb3, 0, 0, 0, 0, 0, 0;

0, 0, 0, m4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/2*m4*l4*cb4, 0, 0, 0, 0;

0, 0, 0, 0, m5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/2*m5*l5*cb5, 0, 0;

0, 0, 0, 0, 0, m6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/2*m6*l6*cb6;

0, -1/3*M*l*sq3*sal1*sax*caz-1/3*M*l*sq3*sal1*cax*say*saz, 0, 0, 0, 0, JX*cay^2*caz^2-1/3*M*l^2*cay^2-

JY*cay^2*caz^2+1/3*M*l^2*cay^2*caz^2+JZ+1/3*M*l^2+JY*cay^2-JZ*cay^2, JX*cay*saz*caz-JY*cay*saz*caz+1/3*M*l^2*cay*saz*caz,

1/3*M*l^2*say+JZ*say, 0, 0, 1/3*M*l2*l*sq3*cb2*sax*caz*cal1*sa2-

1/3*M*l2*l*sq3*ca2*cb2*sax*say*saz+1/3*M*l2*l*sq3*ca2*cb2*cax*caz+1/3*M*l2*l*sq3*cb2*saz*cax*sa2*say*cal1, -

1/3*M*l2*l*sq3*sa2*sb2*cax*caz+1/3*M*l2*l*sq3*sb2*sax*caz*cal1*ca2+1/3*M*l2*l*sq3*sb2*saz*cax*ca2*say*cal1+1/3*M*l2*l*sq3*sa

l1*cb2*sax*caz+1/3*M*l2*l*sq3*sal1*cb2*cax*say*saz+1/3*M*l2*l*sq3*sa2*sb2*sax*say*saz, 0, 0, 0, 0, 0, 0, 0, 0;

0, 1/3*M*l*sq3*cal1*say*saz-1/3*M*l*sq3*sal1*saz*sax*cay, 0, 0, 0, 0, JX*cay*saz*caz-

JY*cay*saz*caz+1/3*M*l^2*cay*saz*caz, JX-1/3*M*l^2*caz^2-JX*caz^2+JY*caz^2+1/3*M*l^2, 0, 0, 0,

11-11 | P a g e

1/3*M*l2*l*sq3*ca2*cb2*saz*cax*cay+1/3*M*l2*l*sq3*cb2*say*saz*sal1*sa2+1/3*M*l2*l*sq3*cb2*saz*sax*cay*cal1*sa2, -

1/3*M*l2*l*sq3*cal1*cb2*say*saz+1/3*M*l2*l*sq3*sb2*saz*sax*ca2*cay*cal1+1/3*M*l2*l*sq3*sb2*say*saz*sal1*ca2+1/3*M*l2*l*sq3*s

al1*cb2*saz*sax*cay-1/3*M*l2*l*sq3*sa2*sb2*saz*cax*cay, 0, 0, 0, 0, 0, 0, 0, 0;

0, -1/3*M*l*sq3*cay*caz*cal1-1/3*M*l*sq3*cax*saz*sal1-1/3*M*l*sq3*sax*say*caz*sal1, 0, 0, 0, 0, 1/3*M*l^2*say+JZ*say, 0,

JZ+1/3*M*l^2, 0, 0, 1/3*M*l2*l*sq3*cb2*sax*say*caz*cal1*sa2+1/3*M*l2*l*sq3*ca2*cb2*cax*say*caz-

1/3*M*l2*l*sq3*cb2*cay*caz*sal1*sa2+1/3*M*l2*l*sq3*cb2*cax*saz*cal1*sa2-1/3*M*l2*l*sq3*ca2*cb2*sax*saz,

1/3*M*l2*l*sq3*cax*saz*sal1*cb2-

1/3*M*l2*l*sq3*cay*caz*sal1*ca2*sb2+1/3*M*l2*l*sq3*cay*caz*cal1*cb2+1/3*M*l2*l*sq3*cax*saz*cal1*ca2*sb2+1/3*M*l2*l*sq3*sax*s

az*sa2*sb2+1/3*M*l2*l*sq3*sax*say*caz*sal1*cb2+1/3*M*l2*l*sq3*sax*say*caz*cal1*ca2*sb2-1/3*M*l2*l*sq3*cax*say*caz*sa2*sb2,

0, 0, 0, 0, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m1*l1^2*cb1^2+Jx*cb1^2-Jy*cb1^2+Jy, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

-1/2*m1*l1*cb1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m1*l1^2+Jz, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, 1/3*M*l2*l*sq3*cb2*sax*caz*cal1*sa2-

1/3*M*l2*l*sq3*ca2*cb2*sax*say*saz+1/3*M*l2*l*sq3*ca2*cb2*cax*caz+1/3*M*l2*l*sq3*cb2*saz*cax*sa2*say*cal1,

1/3*M*l2*l*sq3*ca2*cb2*saz*cax*cay+1/3*M*l2*l*sq3*cb2*say*saz*sal1*sa2+1/3*M*l2*l*sq3*cb2*saz*sax*cay*cal1*sa2,

1/3*M*l2*l*sq3*cb2*sax*say*caz*cal1*sa2+1/3*M*l2*l*sq3*ca2*cb2*cax*say*caz-

1/3*M*l2*l*sq3*cb2*cay*caz*sal1*sa2+1/3*M*l2*l*sq3*cb2*cax*saz*cal1*sa2-1/3*M*l2*l*sq3*ca2*cb2*sax*saz, 0, 0, Jx*cb2^2-

Jy*cb2^2+Jy+1/4*m2*l2^2*cb2^2+M*l2^2*cb2^2, 0, 0, 0, 0, 0, 0, 0, 0, 0;

0, -1/2*m2*l2*cb2-M*l2*cb2, 0, 0, 0, 0, -

1/3*M*l2*l*sq3*sa2*sb2*cax*caz+1/3*M*l2*l*sq3*sb2*sax*caz*cal1*ca2+1/3*M*l2*l*sq3*sb2*saz*cax*ca2*say*cal1+1/3*M*l2*l*sq3*sa

l1*cb2*sax*caz+1/3*M*l2*l*sq3*sal1*cb2*cax*say*saz+1/3*M*l2*l*sq3*sa2*sb2*sax*say*saz, -

1/3*M*l2*l*sq3*cal1*cb2*say*saz+1/3*M*l2*l*sq3*sb2*saz*sax*ca2*cay*cal1+1/3*M*l2*l*sq3*sb2*say*saz*sal1*ca2+1/3*M*l2*l*sq3*s

al1*cb2*saz*sax*cay-1/3*M*l2*l*sq3*sa2*sb2*saz*cax*cay, 1/3*M*l2*l*sq3*cax*saz*sal1*cb2-

1/3*M*l2*l*sq3*cay*caz*sal1*ca2*sb2+1/3*M*l2*l*sq3*cay*caz*cal1*cb2+1/3*M*l2*l*sq3*cax*saz*cal1*ca2*sb2+1/3*M*l2*l*sq3*sax*s

az*sa2*sb2+1/3*M*l2*l*sq3*sax*say*caz*sal1*cb2+1/3*M*l2*l*sq3*sax*say*caz*cal1*ca2*sb2-1/3*M*l2*l*sq3*cax*say*caz*sa2*sb2,

0, 0, 0, 1/4*m2*l2^2+M*l2^2+Jz, 0, 0, 0, 0, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m3*l3^2*cb3^2+Jx*cb3^2-Jy*cb3^2+Jy, 0, 0, 0, 0, 0, 0, 0;

0, 0, -1/2*m3*l3*cb3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m3*l3^2+Jz, 0, 0, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m4*l4^2*cb4^2+Jy+Jx*cb4^2-Jy*cb4^2, 0, 0, 0, 0, 0;

0, 0, 0, -1/2*m4*l4*cb4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, Jz+1/4*m4*l4^2, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, Jy+1/4*m5*l5^2*cb5^2+Jx*cb5^2-Jy*cb5^2, 0, 0, 0;

0, 0, 0, 0, -1/2*m5*l5*cb5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, Jz+1/4*m5*l5^2, 0, 0;

0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, Jy+1/4*m6*l6^2*cb6^2+Jx*cb6^2-Jy*cb6^2, 0;

0, 0, 0, 0, 0, -1/2*m6*l6*cb6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m6*l6^2+Jz];


xC=[0, 0, 0, 0, 0, 0, -cal1, -sal1, 0, 0, 0, 0, 0, 0, 0;

-cal1, -sal1, 0, -cal1, -sal1, 0, cal1, sal1, 0, 0, 0, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, 0, 0, 0, -cal2, -sal2, 0, 0, 0, 0;

cal2, sal2, 0, 0, 0, 0, 0, 0, 0, cal2, sal2, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -cal3, -sal3, 0;

0, 0, 0, cal3, sal3, 0, 0, 0, 0, 0, 0, 0, cal3, sal3, 0;

0, -1/2*l*(cax*say*caz-sax*saz-sq3*cax*say*saz-sq3*sax*caz), -1/2*l*(sax*say*caz+cax*saz-sq3*sax*say*saz+sq3*cax*caz), 0,

1/2*l*(cax*say*caz-sax*saz+sq3*cax*say*saz+sq3*sax*caz), -1/2*l*(-sax*say*caz-cax*saz-sq3*sax*say*saz+sq3*cax*caz), 0, 0, 0,

0, 0, 0, 0, 0, 0;

1/2*say*l*(caz-sq3*saz), -1/2*l*sax*cay*(caz-sq3*saz), 1/2*l*cax*cay*(caz-sq3*saz), -1/2*say*l*(caz+sq3*saz),

1/2*l*sax*cay*(caz+sq3*saz), -1/2*l*cax*cay*(caz+sq3*saz), 0, 0, 0, 0, 0, 0, 0, 0, 0;

1/2*cay*l*(saz+sq3*caz), -1/2*l*(-sax*say*saz+cax*caz-sq3*sax*say*caz-sq3*cax*saz), -

1/2*l*(cax*say*saz+sax*caz+sq3*cax*say*caz-sq3*sax*saz), 1/2*cay*l*(-saz+sq3*caz), 1/2*l*(-

sax*say*saz+cax*caz+sq3*sax*say*caz+sq3*cax*saz), -1/2*l*(-cax*say*saz-sax*caz+sq3*cax*say*caz-sq3*sax*saz), 0, 0, 0, 0, 0,

0, 0, 0, 0;

0, 0, 0, 0, 0, 0, -l1*sal1*sa1*cb1, l1*cal1*sa1*cb1, -ca1*cb1*l1, 0, 0, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, -l1*(-cal1*cb1+sal1*ca1*sb1), l1*(sal1*cb1+cal1*ca1*sb1), sa1*sb1*l1, 0, 0, 0, 0, 0, 0;

-l2*sal1*sa2*cb2, l2*cal1*sa2*cb2, -ca2*cb2*l2, -l2*sal1*sa2*cb2, l2*cal1*sa2*cb2, -ca2*cb2*l2, l2*sal1*sa2*cb2, -

l2*cal1*sa2*cb2, ca2*cb2*l2, 0, 0, 0, 0, 0, 0;

-l2*(-cal1*cb2+sal1*ca2*sb2), l2*(sal1*cb2+cal1*ca2*sb2), sa2*sb2*l2, -l2*(-cal1*cb2+sal1*ca2*sb2),

l2*(sal1*cb2+cal1*ca2*sb2), sa2*sb2*l2, l2*(-cal1*cb2+sal1*ca2*sb2), -l2*(sal1*cb2+cal1*ca2*sb2), -sa2*sb2*l2, 0, 0, 0, 0,

0, 0;

11-12 | P a g e

0, 0, 0, 0, 0, 0, 0, 0, 0, -l3*sal2*sa3*cb3, l3*cal2*sa3*cb3, -ca3*cb3*l3, 0, 0, 0;

0, 0, 0, 0, 0, 0, 0, 0, 0, -l3*(-cal2*cb3+sal2*ca3*sb3), l3*(sal2*cb3+cal2*ca3*sb3), sa3*sb3*l3, 0, 0, 0;

l4*sal2*sa4*cb4, -l4*cal2*sa4*cb4, ca4*cb4*l4, 0, 0, 0, 0, 0, 0, l4*sal2*sa4*cb4, -l4*cal2*sa4*cb4, ca4*cb4*l4, 0, 0, 0;

l4*(-cal2*cb4+sal2*ca4*sb4), -l4*(sal2*cb4+cal2*ca4*sb4), -sa4*sb4*l4, 0, 0, 0, 0, 0, 0, l4*(-cal2*cb4+sal2*ca4*sb4), -

l4*(sal2*cb4+cal2*ca4*sb4), -sa4*sb4*l4, 0, 0, 0;

0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -l5*sal3*sa5*cb5, l5*cal3*sa5*cb5, -ca5*cb5*l5;

0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -l5*(-cal3*cb5+sal3*ca5*sb5), l5*(sal3*cb5+cal3*ca5*sb5), sa5*sb5*l5;

0, 0, 0, l6*sal3*sa6*cb6, -l6*cal3*sa6*cb6, ca6*cb6*l6, 0, 0, 0, 0, 0, 0, l6*sal3*sa6*cb6, -l6*cal3*sa6*cb6, ca6*cb6*l6;

0, 0, 0, -l6*(cal3*cb6-sal3*ca6*sb6), -l6*(sal3*cb6+cal3*ca6*sb6), -sa6*sb6*l6, 0, 0, 0, 0, 0, 0, -l6*(cal3*cb6-

sal3*ca6*sb6), -l6*(sal3*cb6+cal3*ca6*sb6), -sa6*sb6*l6]';

11.6.2.4. Torque Dissipation Matrix

E=[eye(6), zeros(15, 6)];



vF=[0;

0;

M*g;

-1/2*m1*l1*cb1*bd1^2;

-1/2*m2*l2*cb2*bd2^2;

-1/2*m3*l3*bd3^2*cb3;

-1/2*m4*l4*bd4^2*cb4;

-1/2*m5*l5*bd5^2*cb5;

-1/2*m6*l6*bd6^2*cb6;

0;

0;

0;

1/2*m1*l1^2*ad1*bd1*sb1*cb1-2*ad1*Jy*bd1*sb1*cb1+2*ad1*Jz*bd1*sb1*cb1+1/2*m1*g*l1*ca1*sb1;

-1/4*m1*l1^2*ad1^2*sb1*cb1+1/2*m1*g*l1*cb1*sa1+ad1^2*Jy*sb1*cb1-ad1^2*Jz*sb1*cb1;


1/2*m2*g*l2*sa2*cb2-1/4*m2*l2^2*ad2^2*sb2*cb2+ad2^2*Jy*sb2*cb2-ad2^2*Jz*sb2*cb2;


ad3^2*Jy*sb3*cb3-ad3^2*Jz*sb3*cb3+1/2*m3*g*l3*sa3*cb3-1/4*m3*l3^2*ad3^2*sb3*cb3;

2*ad4*Jz*bd4*sb4*cb4-2*ad4*Jy*bd4*sb4*cb4+1/2*m4*l4^2*ad4*bd4*sb4*cb4+1/2*m4*g*l4*ca4*sb4;

-ad4^2*Jz*sb4*cb4+ad4^2*Jy*sb4*cb4+1/2*m4*g*l4*sa4*cb4-1/4*m4*l4^2*ad4^2*sb4*cb4;

-2*ad5*Jy*bd5*sb5*cb5+2*ad5*Jz*bd5*sb5*cb5+1/2*m5*l5^2*ad5*bd5*sb5*cb5+1/2*m5*g*l5*ca5*sb5;

1/2*m5*g*l5*sa5*cb5-1/4*m5*l5^2*ad5^2*sb5*cb5+ad5^2*Jy*sb5*cb5-ad5^2*Jz*sb5*cb5;


1/2*m6*g*l6*sa6*cb6-1/4*m6*l6^2*ad6^2*sb6*cb6+ad6^2*Jy*sb6*cb6-ad6^2*Jz*sb6*cb6];


xM=[M, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

0, M, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

0, 0, M, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

0, 0, 0, m1, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/2*m1*l1*sb1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

0, 0, 0, 0, m2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/2*m2*l2*sb2, 0, 0, 0, 0, 0, 0, 0, 0;

0, 0, 0, 0, 0, m3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/2*m3*l3*sb3, 0, 0, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, m4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/2*m4*l4*sb4, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, 0, m5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/2*m5*l5*sb5, 0, 0;

0, 0, 0, 0, 0, 0, 0, 0, m6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/2*m6*l6*sb6;

0, 0, 0, 0, 0, 0, 0, 0, 0, JX, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, 0, 0, 0, 0, JY, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, JZ, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, Jz-1/4*m1*l1^2*cb1^2+1/4*m1*l1^2+Jy*cb1^2-Jz*cb1^2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

11-13 | P a g e

0, 0, 0, -1/2*m1*l1*sb1, 0, 0, 0, 0, 0, 0, 0, 0, 0, Jx+1/4*m1*l1^2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, Jz-1/4*m2*l2^2*cb2^2+1/4*m2*l2^2+Jy*cb2^2-Jz*cb2^2, 0, 0, 0, 0, 0, 0, 0, 0, 0;

0, 0, 0, 0, -1/2*m2*l2*sb2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m2*l2^2+Jx, 0, 0, 0, 0, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/4*m3*l3^2*cb3^2+1/4*m3*l3^2+Jy*cb3^2-Jz*cb3^2+Jz, 0, 0, 0, 0, 0, 0, 0;

0, 0, 0, 0, 0, -1/2*m3*l3*sb3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m3*l3^2+Jx, 0, 0, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -Jz*cb4^2+Jz+Jy*cb4^2+1/4*m4*l4^2-1/4*m4*l4^2*cb4^2, 0, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, -1/2*m4*l4*sb4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m4*l4^2+Jx, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m5*l5^2+Jy*cb5^2-Jz*cb5^2+Jz-1/4*m5*l5^2*cb5^2, 0, 0, 0;

0, 0, 0, 0, 0, 0, 0, -1/2*m5*l5*sb5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, Jx+1/4*m5*l5^2, 0, 0;

0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/4*m6*l6^2*cb6^2+1/4*m6*l6^2+Jy*cb6^2-Jz*cb6^2+Jz, 0;

0, 0, 0, 0, 0, 0, 0, 0, -1/2*m6*l6*sb6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m6*l6^2+Jx];


xC=[0, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 0, -1, 0, 0, -1, 0, 0;

0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 0, -1, 0, 0, -1, 0;

0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 0, -1, 0, 0, -1;

cal1, sal1, 0, 0, 0, 0, 0, 0, 0, cal1, sal1, 0, 0, 0, 0, 0, 0, 0;

-cal1, -sal1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

0, 0, 0, cal2, sal2, 0, 0, 0, 0, 0, 0, 0, cal2, sal2, 0, 0, 0, 0;

0, 0, 0, -cal2, -sal2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, cal3, sal3, 0, 0, 0, 0, 0, 0, 0, cal3, sal3, 0;

0, 0, 0, 0, 0, 0, -cal3, -sal3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/3*l*3^(1/2)*cax*say*saz-1/3*l*3^(1/2)*sax*caz, -

1/3*l*3^(1/2)*sax*say*saz+1/3*l*3^(1/2)*cax*caz, 0, -1/3*l*3^(1/2)*cax*say*saz-1/3*l*3^(1/2)*sax*caz, -

1/3*l*3^(1/2)*sax*say*saz+1/3*l*3^(1/2)*cax*caz, 0, -1/3*l*3^(1/2)*cax*say*saz-1/3*l*3^(1/2)*sax*caz, -

1/3*l*3^(1/2)*sax*say*saz+1/3*l*3^(1/2)*cax*caz;

0, 0, 0, 0, 0, 0, 0, 0, 0, 1/3*l*3^(1/2)*say*saz, -1/3*l*3^(1/2)*saz*sax*cay, 1/3*l*3^(1/2)*saz*cax*cay,

1/3*l*3^(1/2)*say*saz, -1/3*l*3^(1/2)*saz*sax*cay, 1/3*l*3^(1/2)*saz*cax*cay, 1/3*l*3^(1/2)*say*saz, -

1/3*l*3^(1/2)*saz*sax*cay, 1/3*l*3^(1/2)*saz*cax*cay;

0, 0, 0, 0, 0, 0, 0, 0, 0, -1/3*l*3^(1/2)*cay*caz, -1/3*l*3^(1/2)*sax*say*caz-1/3*l*3^(1/2)*cax*saz,

1/3*l*3^(1/2)*cax*say*caz-1/3*l*3^(1/2)*sax*saz, -1/3*l*3^(1/2)*cay*caz, -1/3*l*3^(1/2)*sax*say*caz-1/3*l*3^(1/2)*cax*saz,

1/3*l*3^(1/2)*cax*say*caz-1/3*l*3^(1/2)*sax*saz, -1/3*l*3^(1/2)*cay*caz, -1/3*l*3^(1/2)*sax*say*caz-1/3*l*3^(1/2)*cax*saz,

1/3*l*3^(1/2)*cax*say*caz-1/3*l*3^(1/2)*sax*saz;

l1*sb1*sal1*sa1, -l1*sb1*cal1*sa1, l1*sb1*ca1, 0, 0, 0, 0, 0, 0, l1*sb1*sal1*sa1, -l1*sb1*cal1*sa1, l1*sb1*ca1, 0, 0, 0,

0, 0, 0;

-l1*cal1*sb1-l1*cb1*sal1*ca1, -l1*sal1*sb1+l1*cb1*cal1*ca1, l1*cb1*sa1, 0, 0, 0, 0, 0, 0, -l1*cal1*sb1-l1*cb1*sal1*ca1, -

l1*sal1*sb1+l1*cb1*cal1*ca1, l1*cb1*sa1, 0, 0, 0, 0, 0, 0;

-l2*sb2*sal1*sa2, l2*sb2*cal1*sa2, -l2*ca2*sb2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

l2*cal1*sb2+l2*cb2*sal1*ca2, l2*sal1*sb2-l2*cb2*cal1*ca2, -l2*sa2*cb2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

0, 0, 0, l3*sb3*sal2*sa3, -l3*sb3*cal2*sa3, l3*ca3*sb3, 0, 0, 0, 0, 0, 0, l3*sb3*sal2*sa3, -l3*sb3*cal2*sa3, l3*ca3*sb3,

0, 0, 0;

0, 0, 0, -l3*cal2*sb3-l3*cb3*sal2*ca3, -l3*sal2*sb3+l3*cb3*cal2*ca3, l3*sa3*cb3, 0, 0, 0, 0, 0, 0, -l3*cal2*sb3-

l3*cb3*sal2*ca3, -l3*sal2*sb3+l3*cb3*cal2*ca3, l3*sa3*cb3, 0, 0, 0;

0, 0, 0, -l4*sb4*sal2*sa4, l4*sb4*cal2*sa4, -l4*ca4*sb4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

0, 0, 0, l4*cal2*sb4+l4*cb4*sal2*ca4, l4*sal2*sb4-l4*cb4*cal2*ca4, -l4*sa4*cb4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, l5*sb5*sal3*sa5, -l5*sb5*cal3*sa5, l5*ca5*sb5, 0, 0, 0, 0, 0, 0, l5*sb5*sal3*sa5, -l5*sb5*cal3*sa5,

l5*ca5*sb5;

0, 0, 0, 0, 0, 0, -l5*cal3*sb5-l5*cb5*sal3*ca5, -l5*sal3*sb5+l5*cb5*cal3*ca5, l5*sa5*cb5, 0, 0, 0, 0, 0, 0, -l5*cal3*sb5-

l5*cb5*sal3*ca5, -l5*sal3*sb5+l5*cb5*cal3*ca5, l5*sa5*cb5;

0, 0, 0, 0, 0, 0, -l6*sb6*sal3*sa6, l6*sb6*cal3*sa6, -l6*ca6*sb6, 0, 0, 0, 0, 0, 0, 0, 0, 0;

0, 0, 0, 0, 0, 0, l6*cal3*sb6+l6*cb6*sal3*ca6, l6*sal3*sb6-l6*cb6*cal3*ca6, -l6*sa6*cb6, 0, 0, 0, 0, 0, 0, 0, 0, 0]’;

11.6.3.4. Torque Dissipation Matrix

E=[zeros(3,6), eye(6), zeros(15, 6)];

11-14 | P a g e

11.6.4. 6 joint variables + 6 task variables set


xC=[-2*sal1*ye-2*cal1*xe+2*sal1*ya1+2*cal1*xa1+2*s1-2/3*sal1*l*3^(1/2)*sax*say*saz-

2/3*cal1*cay*saz*l*3^(1/2)+2/3*sal1*l*3^(1/2)*cax*caz,0,0,0,0,0;

0,2*cal1*xa1-2*sal1*ye+2*sal1*ya1+2*s2-2/3*sal1*l*3^(1/2)*sax*say*saz-2/3*cal1*cay*saz*l*3^(1/2)-

2*cal1*xe+2/3*sal1*l*3^(1/2)*cax*caz,0,0,0,0;

0,0,2*cal2*xa2+2*s3+1/3*cal2*cay*saz*l*3^(1/2)-sal2*l*sax*say*caz-1/3*sal2*l*3^(1/2)*cax*caz+2*sal2*ya2-2*sal2*ye-

2*cal2*xe-cal2*cay*caz*l-sal2*l*cax*saz+1/3*sal2*l*3^(1/2)*sax*say*saz,0,0,0;

0,0,0,2*cal2*xa2-2*cal2*xe+2*s4+1/3*cal2*cay*saz*l*3^(1/2)-sal2*l*sax*say*caz-1/3*sal2*l*3^(1/2)*cax*caz+2*sal2*ya2-

2*sal2*ye-cal2*cay*caz*l-sal2*l*cax*saz+1/3*sal2*l*3^(1/2)*sax*say*saz,0,0;

0,0,0,0,2*cal3*xa3+1/3*cal3*cay*saz*l*3^(1/2)+sal3*l*sax*say*caz-1/3*sal3*l*3^(1/2)*cax*caz-2*sal3*ye+2*sal3*ya3-

2*cal3*xe+2*s5+cal3*cay*caz*l+sal3*l*cax*saz+1/3*sal3*l*3^(1/2)*sax*say*saz,0;

0,0,0,0,0,-2*cal3*xe+1/3*sal3*l*3^(1/2)*sax*say*saz+2*s6+1/3*cal3*cay*saz*l*3^(1/2)+sal3*l*sax*say*caz-

1/3*sal3*l*3^(1/2)*cax*caz-2*sal3*ye+2*sal3*ya3+2*cal3*xa3+cal3*cay*caz*l+sal3*l*cax*saz;

-2*cal1*s1+2*xe-2*xa1+2/3*cay*saz*l*3^(1/2),2*xe-2*xa1+2/3*cay*saz*l*3^(1/2)-2*cal1*s2,2*xe-2*xa2-

1/3*cay*saz*l*3^(1/2)+cay*caz*l-2*cal2*s3,-2*cal2*s4+2*xe-2*xa2-1/3*cay*saz*l*3^(1/2)+cay*caz*l,2*xe-2*xa3-

1/3*cay*saz*l*3^(1/2)-cay*caz*l-2*cal3*s5,2*xe-2*xa3-1/3*cay*saz*l*3^(1/2)-cay*caz*l-2*cal3*s6;

-2*sal1*s1+2/3*l*3^(1/2)*sax*say*saz+2*ye-2*ya1-2/3*l*3^(1/2)*cax*caz,-2*sal1*s2+2/3*l*3^(1/2)*sax*say*saz+2*ye-2*ya1-

2/3*l*3^(1/2)*cax*caz,-1/3*l*3^(1/2)*sax*say*saz+2*ye-2*ya2+l*sax*say*caz+l*cax*saz-2*sal2*s3+1/3*l*3^(1/2)*cax*caz,-

2*sal2*s4-1/3*l*3^(1/2)*sax*say*saz+2*ye-2*ya2+l*sax*say*caz+l*cax*saz+1/3*l*3^(1/2)*cax*caz,-

1/3*l*3^(1/2)*sax*say*saz+2*ye-2*ya3-l*sax*say*caz-l*cax*saz-2*sal3*s5+1/3*l*3^(1/2)*cax*caz,-2*sal3*s6-

1/3*l*3^(1/2)*sax*say*saz+2*ye-2*ya3-l*sax*say*caz-l*cax*saz+1/3*l*3^(1/2)*cax*caz;

-2/3*l*3^(1/2)*cax*say*saz+2*ze-2/3*l*3^(1/2)*sax*caz,-2/3*l*3^(1/2)*cax*say*saz+2*ze-

2/3*l*3^(1/2)*sax*caz,1/3*l*3^(1/2)*cax*say*saz+2*ze-

l*cax*say*caz+l*sax*saz+1/3*l*3^(1/2)*sax*caz,1/3*l*3^(1/2)*cax*say*saz+2*ze-

l*cax*say*caz+l*sax*saz+1/3*l*3^(1/2)*sax*caz,1/3*l*3^(1/2)*cax*say*saz+2*ze+l*cax*say*caz-

l*sax*saz+1/3*l*3^(1/2)*sax*caz,1/3*l*3^(1/2)*cax*say*saz+2*ze+l*cax*say*caz-l*sax*saz+1/3*l*3^(1/2)*sax*caz;

-2/3*l*3^(1/2)*(-sax*caz*ye+cax*caz*ze-cax*say*saz*ye-

sax*say*saz*ze+cax*say*saz*ya1+sal1*s1*sax*caz+sax*caz*ya1+sal1*s1*cax*say*saz),-2/3*l*3^(1/2)*(-sax*caz*ye+cax*caz*ze-

cax*say*saz*ye-sax*say*saz*ze+cax*say*saz*ya1+sax*caz*sal1*s2+cax*say*saz*sal1*s2+sax*caz*ya1),-1/3*l*(-3*cax*say*caz*ye-

3*sax*say*caz*ze+3^(1/2)*sax*caz*ye-3^(1/2)*cax*caz*ze+3^(1/2)*cax*say*saz*ye+3^(1/2)*sax*say*saz*ze+3*sal2*s3*cax*say*caz-

sal2*s3*3^(1/2)*sax*caz-sal2*s3*3^(1/2)*cax*say*saz+3*sax*saz*ye-3*cax*saz*ze-3*sal2*s3*sax*saz+3*cax*say*caz*ya2-

ya2*3^(1/2)*sax*caz-3*sax*saz*ya2-ya2*3^(1/2)*cax*say*saz),-1/3*l*(-3*cax*say*caz*ye-3*sax*say*caz*ze+3^(1/2)*sax*caz*ye-

3^(1/2)*cax*caz*ze+3^(1/2)*cax*say*saz*ye+3^(1/2)*sax*say*saz*ze+3*sax*saz*ye-3*cax*saz*ze+3*cax*say*caz*ya2-

ya2*3^(1/2)*sax*caz-3*sax*saz*sal2*s4-sal2*s4*3^(1/2)*cax*say*saz-3*sax*saz*ya2+3*cax*say*caz*sal2*s4-

sal2*s4*3^(1/2)*sax*caz-ya2*3^(1/2)*cax*say*saz),1/3*l*(-3*cax*say*caz*ye-3*sax*say*caz*ze-

3^(1/2)*sax*caz*ye+3^(1/2)*cax*caz*ze-3^(1/2)*cax*say*saz*ye-3^(1/2)*sax*say*saz*ze+ya3*3^(1/2)*cax*say*saz+3*sax*saz*ye-

3*cax*saz*ze+3*cax*say*caz*ya3+ya3*3^(1/2)*sax*caz-3*sal3*s5*sax*saz+sal3*s5*3^(1/2)*cax*say*saz-

3*sax*saz*ya3+3*sal3*s5*cax*say*caz+sal3*s5*3^(1/2)*sax*caz),1/3*l*(-3*cax*say*caz*ye-3*sax*say*caz*ze-

3^(1/2)*sax*caz*ye+3^(1/2)*cax*caz*ze-3^(1/2)*cax*say*saz*ye-

3^(1/2)*sax*say*saz*ze+ya3*3^(1/2)*cax*say*saz+3*cax*say*caz*sal3*s6+3*sax*saz*ye-

3*cax*saz*ze+3*cax*say*caz*ya3+ya3*3^(1/2)*sax*caz-3*sax*saz*sal3*s6+sal3*s6*3^(1/2)*cax*say*saz-

3*sax*saz*ya3+sal3*s6*3^(1/2)*sax*caz);

-2/3*saz*l*3^(1/2)*(say*xe-sax*cay*ye+cax*cay*ze+sax*cay*ya1-cal1*s1*say-say*xa1+sal1*s1*sax*cay),-

2/3*saz*l*3^(1/2)*(say*xe-sax*cay*ye+cax*cay*ze+sax*cay*ya1-say*cal1*s2+sax*cay*sal1*s2-say*xa1),-1/3*l*(-

3*sax*cay*caz*ye+3*cax*cay*caz*ze-say*saz*3^(1/2)*xe+3^(1/2)*sax*cay*saz*ye-

3^(1/2)*cax*cay*saz*ze+cal2*s3*say*saz*3^(1/2)+3*sal2*s3*sax*cay*caz-sal2*s3*3^(1/2)*sax*cay*saz+3*say*caz*xe-

3*cal2*s3*say*caz+xa2*say*saz*3^(1/2)+3*ya2*sax*cay*caz-3*xa2*say*caz-ya2*3^(1/2)*sax*cay*saz),-1/3*l*(-

3*sax*cay*caz*ye+3*cax*cay*caz*ze-say*saz*3^(1/2)*xe+3^(1/2)*sax*cay*saz*ye-

3^(1/2)*cax*cay*saz*ze+3*say*caz*xe+xa2*say*saz*3^(1/2)+3*ya2*sax*cay*caz-3*cal2*s4*say*caz-sal2*s4*3^(1/2)*sax*cay*saz-

3*xa2*say*caz+cal2*s4*say*saz*3^(1/2)+3*sal2*s4*sax*cay*caz-ya2*3^(1/2)*sax*cay*saz),1/3*l*(-

3*sax*cay*caz*ye+3*cax*cay*caz*ze+say*saz*3^(1/2)*xe-

3^(1/2)*sax*cay*saz*ye+3^(1/2)*cax*cay*saz*ze+ya3*3^(1/2)*sax*cay*saz+3*say*caz*xe-xa3*say*saz*3^(1/2)+3*ya3*sax*cay*caz-

3*cal3*s5*say*caz+sal3*s5*3^(1/2)*sax*cay*saz-3*xa3*say*caz-cal3*s5*say*saz*3^(1/2)+3*sal3*s5*sax*cay*caz),1/3*l*(-

3*sax*cay*caz*ye+3*cax*cay*caz*ze+say*saz*3^(1/2)*xe-3^(1/2)*sax*cay*saz*ye+3^(1/2)*cax*cay*saz*ze+ya3*3^(1/2)*sax*cay*saz-

11-15 | P a g e

cal3*s6*say*saz*3^(1/2)+3*say*caz*xe-xa3*say*saz*3^(1/2)+3*ya3*sax*cay*caz-3*cal3*s6*say*caz+sal3*s6*3^(1/2)*sax*cay*saz-

3*xa3*say*caz+3*sal3*s6*sax*cay*caz);

-2/3*l*3^(1/2)*(-cay*caz*xe-cax*saz*ye-sax*saz*ze-

sax*say*caz*ye+cax*say*caz*ze+sax*say*caz*ya1+cal1*s1*cay*caz+sal1*s1*cax*saz+cay*caz*xa1+cax*saz*ya1+sal1*s1*sax*say*caz),-

2/3*l*3^(1/2)*(-cay*caz*xe-cax*saz*ye-sax*saz*ze-

sax*say*caz*ye+cax*say*caz*ze+sax*say*caz*ya1+cay*caz*cal1*s2+cax*saz*sal1*s2+sax*say*caz*sal1*s2+cay*caz*xa1+cax*saz*ya1),1

/3*l*(-3*sax*say*saz*ye+3*cax*say*saz*ze-cay*caz*3^(1/2)*xe-3^(1/2)*cax*saz*ye-3^(1/2)*sax*saz*ze-

3^(1/2)*sax*say*caz*ye+3^(1/2)*cax*say*caz*ze+cal2*s3*cay*caz*3^(1/2)+3*sal2*s3*sax*say*saz+sal2*s3*3^(1/2)*cax*saz+sal2*s3*

3^(1/2)*sax*say*caz-3*cay*saz*xe+3*cax*caz*ye+3*sax*caz*ze+3*cal2*s3*cay*saz-

3*sal2*s3*cax*caz+xa2*cay*caz*3^(1/2)+3*sax*say*saz*ya2+ya2*3^(1/2)*cax*saz+3*cay*saz*xa2-

3*cax*caz*ya2+ya2*3^(1/2)*sax*say*caz),1/3*l*(-3*sax*say*saz*ye+3*cax*say*saz*ze-cay*caz*3^(1/2)*xe-3^(1/2)*cax*saz*ye-

3^(1/2)*sax*saz*ze-3^(1/2)*sax*say*caz*ye+3^(1/2)*cax*say*caz*ze-

3*cay*saz*xe+3*cax*caz*ye+3*sax*caz*ze+xa2*cay*caz*3^(1/2)+3*sax*say*saz*ya2+ya2*3^(1/2)*cax*saz+3*cay*saz*cal2*s4-

3*cax*caz*sal2*s4+sal2*s4*3^(1/2)*sax*say*caz+3*cay*saz*xa2-

3*cax*caz*ya2+cal2*s4*cay*caz*3^(1/2)+3*sax*say*saz*sal2*s4+sal2*s4*3^(1/2)*cax*saz+ya2*3^(1/2)*sax*say*caz),1/3*l*(3*sax*sa

y*saz*ye-3*cax*say*saz*ze-cay*caz*3^(1/2)*xe-3^(1/2)*cax*saz*ye-3^(1/2)*sax*saz*ze-

3^(1/2)*sax*say*caz*ye+3^(1/2)*cax*say*caz*ze+ya3*3^(1/2)*sax*say*caz+3*cay*saz*xe-3*cax*caz*ye-

3*sax*caz*ze+xa3*cay*caz*3^(1/2)-3*sax*say*saz*ya3+ya3*3^(1/2)*cax*saz-

3*cal3*s5*cay*saz+3*sal3*s5*cax*caz+sal3*s5*3^(1/2)*sax*say*caz-3*cay*saz*xa3+3*cax*caz*ya3+cal3*s5*cay*caz*3^(1/2)-

3*sal3*s5*sax*say*saz+sal3*s5*3^(1/2)*cax*saz),1/3*l*(3*sax*say*saz*ye-3*cax*say*saz*ze-cay*caz*3^(1/2)*xe-

3^(1/2)*cax*saz*ye-3^(1/2)*sax*saz*ze-

3^(1/2)*sax*say*caz*ye+3^(1/2)*cax*say*caz*ze+ya3*3^(1/2)*sax*say*caz+cal3*s6*cay*caz*3^(1/2)+3*cay*saz*xe-3*cax*caz*ye-

3*sax*caz*ze+xa3*cay*caz*3^(1/2)-3*sax*say*saz*ya3+ya3*3^(1/2)*cax*saz-

3*cay*saz*cal3*s6+3*cax*caz*sal3*s6+sal3*s6*3^(1/2)*sax*say*caz-3*cay*saz*xa3+3*cax*caz*ya3-

3*sax*say*saz*sal3*s6+sal3*s6*3^(1/2)*cax*saz)];

kinematic, dynamic and workspace analysis of a novel 6 …€¦ · i also thank the armlab members...

Documents