A Hybrid Numerical and Analytical Approach Towards Resolving Loop Closure Constraints in Rigid Body Dynamics

See discussions, stats, and author profiles for this publication at: https://www.researchgate.
net/publication/354533754
A hybrid numerical and analytical approach towards resolving loop closure

constraints in rigid body dynamics
Thesis · August 2021

DOI: 10.13140/RG.2.2.14004.99207
CITATIONS READS
0 361
1 author:
Rohit Kumar
Deutsches Forschungszentrum für Künstliche Intelligenz
8 PUBLICATIONS 11 CITATIONS
SEE PROFILE
All content following this page was uploaded by Rohit Kumar on 08 October 2021.
The user has requested enhancement of the downloaded file.

Università degli Studi di Genova, Italy École Centrale de Nantes, France
MASTER ERASMUS MUNDUS

EMARO+ “European Master in Advanced Robotics”
2020 / 2021
Master Thesis Report
Presented by
Rohit Kumar (Student Id: 5089482)
On August 27, 2021
A hybrid numerical and analytical approach towards

resolving loop closure constraints in rigid body dynamics
Supervisors : Shivesh Kumar (Senior Researcher, DFKI-RIC, Bremen)

Andreas Mueller (Institute of Robotics, JKU, Linz)
Frank Kirchner (Executive Director, DFKI-RIC, Bremen)
Matteo Zoppi (Associate Professor, University of Genova)
Olivier Kermorgant (Associate Professor, École Centrale de Nantes)
Laboratory: Deutsches Forschungszentrum für Künstliche Intelligenz, Robotics Inno-

vation Center, Germany
Acknowledgements
The work done in the thesis has obligation to many who provided constant support in my
amazing journey. I am deeply indebted to each and everyone for the successful completion
of the thesis. It’s a great pleasure to express my gratitude and appreciate those who have
helped me in the process.
Firstly, I would like to thank my first supervisor Dr. rer. nat. Shivesh Kumar for
providing me a great opportunity to work on this interesting topic at DFKI Robotics In-
novation Center. I enjoyed working with him and his guidance truly made me perform
at my best. While working under his supervision, I borrowed great inspiration from his
leadership qualities. I am very grateful to him for providing me the right direction. Be-
sides, he has been a good companion who helped me during my stay in Germany. I would
like to thank him for believing in me and for his efforts to have me as a Master’s thesis
student at DFKI. It is almost impossible to adequately express my gratitude towards him.
Further, I would like to thank my second supervisor Prof. Dr. Andreas Mueller (Institute
of Robotics, Johannes Kepler University, Linz) for providing his constructive feedback and
guidance during my thesis. His work played a major role in enhancing my mathematical
concepts in this field. The discussions with him always helped me to have a better out-
look and visualize things in a bigger context. I would also like to thank Prof. Dr. Frank
Kirchner (Executive Director, DFKI Robotics Innovation Center) for providing adequate
resources to complete my thesis. Moreover, I would like to express my gratitude towards
my co-supervisors Prof. Matteo Zoppi (University of Genoa) and Prof. Olivier Kermorgant
(Ecole Centrale de Nantes) for the required support during the entire Erasmus program.
Secondly, I would like to express my gratitude towards my colleagues Shubham, Melya,

Mihaela, Jonathan, Hauke, Mahdi, Christoph, and others from the under-actuated group
at DFKI Robotics Innovation Center. It was a pleasure to learn and discuss different
concepts. The small projects in this group provided me a hands-on experience with real
systems.
Lastly, I would like to thank my family, maa, papa and didi. The art of expressing
gratitude through words is not adequate for the support they have provided me throughout
my journey. The leanings and guidance from them helped me grow as an individual. Also,
I would like to mention gratitude to my friends Rachna, for her constant support during
the ups and downs in my thesis and Raghu for hosting me when there were visa issues.
I am also grateful to my friends Ashray, Divyendu, Akhil, Preshit, Ravi, Ram, Prabhu,
Akshay, Sahil, Yogesh, Prajwal, and Praveen for some amazing conversations outside my
thesis work.
i
Abstract
Modeling closed loop mechanisms is a necessity for the control and simulation of various
systems like parallel robots, series-parallel hybrid robots, linkages, musculoskeletal systems,
etc. and poses a great challenge to rigid body dynamics algorithms. Many commercial
software (e.g. ADAMS, RecurDyn, Simmechanics, V-Rep etc) and a very limited number
of rigid body dynamics libraries (e.g. RBDL, DARTS, OpenSim ) provides this support.
Solving the equations of motion for rigid body system with closed loops require resolution
of loop closure constraints which are often solved via numerical procedures. This brings an
additional burden to these algorithms as they have to stabilize and control the loop closure
errors additionally. In order to circumvent this issue, in recent work , a kinematics and
dynamics library called Hybrid Robot Dynamics (HyRoDyn) is proposed, which provides a
modular and analytical framework for resolving loop closure constraints in highly complex
series-parallel hybrid robots. HyRoDyn only allows the analytical resolution of loop closure
constraints for the parallel mechanisms that are known to its database.
The work done in the thesis extends the generality of the software significantly as it deals
with the loop closure constraints numerically in a modular way. The user can choose to solve
a submechanism numerically in the HyRoDyn software by providing the loop constraints
in a YAML Ain’t Markup Language (YAML) file. The software architecture involves
parsing of loop closure constraints defined by the user and a new class is implemented to
handle them numerically. A case study of the hybrid series-parallel robot is performed for
validation and results.The thesis also studies a recursive forward dynamics algorithm for
closed loop systems.
The work done in this thesis can be applied to the modeling and control of a complex
biologically inspired series-parallel hybrid robots when the analytical resolutions of loop
closures is not available.
ii
Contents
1 Introduction 1
1.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.1.1 Different Topological Architectures in Robots . . . . . . . . . . . . . 1
1.1.2 Complexity in Modeling of Series-Parallel Hybrid Robots . . . . . . 5
1.2 Objectives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.3 Methodology . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.4 Articulate Body Algorithm with Constraint Embedding . . . . . . . . . . . 7
1.5 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
1.6 Structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2 Modeling of Rigid Body systems 9

2.1 Graph Based Topology . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.1.1 Numbering Scheme . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.1.2 Different Properties of a Graph . . . . . . . . . . . . . . . . . . . . . 10
2.2 Screw Theory and Lie Group Methods . . . . . . . . . . . . . . . . . . . . . 11
2.2.1 Position Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.2.2 Velocity Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.2.3 Acceleration Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.3 Mathematical Background on Constraint Equations of Motion . . . . . . . . 15
2.3.1 Implicit Constraints . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.3.2 Explicit Constraints . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.3.3 Relationship between the Implicit and Explicit Form . . . . . . . . . 17
2.4 Constraint Equations of Motion in Implicit and Explicit Form . . . . . . . . 18
2.5 Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
2.5.1 Vector based Loop Closure Analysis . . . . . . . . . . . . . . . . . . 21
2.5.2 Analysis using Screw Theory . . . . . . . . . . . . . . . . . . . . . . 21
2.5.3 Implicit Form . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
2.5.4 Explicit Form . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
2.5.5 Equivalence of Implicit and Explicit Constraints . . . . . . . . . . . 24
2.5.6 Constraint Equations of Motion in Implicit and Explicit Form . . . 25
iii
3 Numerical Approach for Resolving Loop Closure Equations 27
3.1 Software Dependency on Rigid Body Dynamics Library . . . . . . . . . . . 27
3.2 Mathematical Formulations . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
3.2.1 Extraction of Explicit Jacobian Matrix from Implicit Jacobian Matrix 28
3.2.2 Extraction of Explicit Bias Acceleration from Implicit Bias Acceleration 30
3.3 Example of Lambda Mechanism . . . . . . . . . . . . . . . . . . . . . . . . 31
4 Implementation of Hybrid Numerical and Analytical Approach 33

4.1 A Layer between Implicit and Explicit Form . . . . . . . . . . . . . . . . . . 34
4.2 Further Developments in User Interface . . . . . . . . . . . . . . . . . . . . 34
4.3 Implementation of Hybrid Version in HyRoDyn . . . . . . . . . . . . . . . . 36
4.4 Case Study: Multi Loop Mechanism . . . . . . . . . . . . . . . . . . . . . . 39
4.5 Case Study: Reduced Version of the Upper Body of Humanoid RH5v2 . . . 41
5 Constraint Embedding for Forward Dynamics 44

5.1 Articulated Body Inertia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
5.2 Articulated Body Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
5.3 Introduction to Constraint Embedding . . . . . . . . . . . . . . . . . . . . . 47
5.4 Constraint Embedding in Articulated Body Algorithm . . . . . . . . . . . . 49
5.5 Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
6 Validation and Results 53

6.1 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
6.2 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
6.3 Computational Performance . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
6.4 Validation of Constraint Embedding Approach . . . . . . . . . . . . . . . . 63
7 Conclusions and Future Work 65
A LUA File Extension for Defining the Model 67
References 76
iv
List of Figures
1.1 Serial six degree of freedom manipulator [30] . . . . . . . . . . . . . . . . . 2

1.2 Parallel manipulator [29] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.3 TORO [5] Humanoid robot . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.4 Series-Parallel hybrid robots . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
2.1 Lambda or RRPR mechanism . . . . . . . . . . . . . . . . . . . . . . . . . . 19

2.2 Topological graph for lambda mechanism . . . . . . . . . . . . . . . . . . . 20
2.3 Other possible topological graphs for planar lambda mechanism, adapted
from [6] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.1 Topological graph of RRPR . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
4.1 Extraction of explicit constraints from the implicit constraints . . . . . . . . 34

4.2 NumericalLoopConstraints class in HyRoDyn, adapted from [13] . . . . . . 38
4.3 2SPU+1U mechanism [13] . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
4.4 Topological graph of 2SPU+1U mechanism . . . . . . . . . . . . . . . . . . 39
4.5 Topological graph and submechanism definition of 2SPU+1U mechanism . 40
4.6 Reduced version of RH5v2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
4.7 Topological graph of RRPR mechanism . . . . . . . . . . . . . . . . . . . . 42
4.8 Spanning tree with submechanism definitions . . . . . . . . . . . . . . . . . 43
5.1 Construction of new articulated body, adapted from [6] . . . . . . . . . . . 46

5.2 Kinematic tree with single loop . . . . . . . . . . . . . . . . . . . . . . . . . 47
5.3 Introduction of a submechanism node in the tree, adapted from [10] . . . . 49
5.4 Topological graph of 3R-Lambda-R chain . . . . . . . . . . . . . . . . . . . 51
6.1 Spanning tree of the reduced version of RH5v2 upper body . . . . . . . . . 54

6.2 Position plots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
6.3 Velocity plots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
6.4 Acceleration plots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
6.5 Torque plots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
6.6 Rh5v2 Humanoid robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
v
6.7 Various configurations of RH5v2 . . . . . . . . . . . . . . . . . . . . . . . . 58
6.8 Position plots using experimental data . . . . . . . . . . . . . . . . . . . . . 59
6.9 Velocity plots using experimental data . . . . . . . . . . . . . . . . . . . . . 59
6.10 Torque plots using experimental data . . . . . . . . . . . . . . . . . . . . . . 60
6.11 Comparison of different approaches for resolving loop closure constraints . . 61
6.12 Wrist mechanism in RH5v2 humanoid . . . . . . . . . . . . . . . . . . . . . 62
6.13 Comparison for wrist mechanism . . . . . . . . . . . . . . . . . . . . . . . . 63
6.14 Acceleration plots for constraint embedding validation . . . . . . . . . . . . 64
A.1 RRPR Model using Lua file . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
vi
List of Tables
1.1 Serial v/s Parallel robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.2 Numerical v/s Analytical formulations . . . . . . . . . . . . . . . . . . . . . 6
2.1 Screw coordinates for different joints . . . . . . . . . . . . . . . . . . . . . . 22
6.1 Computation time for analysis . . . . . . . . . . . . . . . . . . . . . . . . . 61

6.2 Computation time where wrist is solved numerically . . . . . . . . . . . . . 63
vii
List of Algorithms
1 calc G from K(K) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

2 calc g from k(K,k) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
3 Articulated body algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
viii
Chapter 1
Introduction
This chapter is dedicated to the overall introduction to the thesis. It introduces the main
topic and its objectives and gives the main idea of the methodology. For a better un-
derstanding, it is divided into sections where Section 1.1 provides the motivation for the
thesis, Section 1.2 illustrates the objectives to be accomplished in the thesis, Section 1.3
gives a brief introduction to the methodology. Section 1.4 gives the approach for a recursive
forward dynamics algorithm. Finally, Section 1.5 discusses the main contribution of the
thesis, and Section 1.6 provides the reference for navigating through the thesis.
1.1 Motivation
The recent developments in the robotics community demand tackling high complexity in
modeling rigid body systems. Robots can be categorized in terms of their topological
structures. A topological structure helps to understand and differentiate between different
robots. This section is divided into two subsections where first provides the introduction
to different topological architectures and the latter discusses the complexity in modeling
of rigid body systems.
1.1.1 Different Topological Architectures in Robots

Serial Robots:
A serial robot can be defined when a base is connected to the end-effector with a series of
rigid links and joints. All the joints are motor actuated.
1
Figure 1.1: Serial six degree of freedom manipulator [30]
In Figure 1.1, a serial robot is shown. The robot has six links that are connected to
each other via a connecting joint. As there are six links, six connecting joints are used to
connect the links, and each connecting joint is actuated by a motor. A connecting joint
can be a revolute joint, prismatic joint, spherical joint, helical joint, etc. For the purpose
of this thesis, the main focus would be on lower pair joints, i.e. revolute, prismatic and
spherical joints. The basic definition of connecting joints can be found in [11, 25, 20].
Parallel Robots:
A parallel robot or a parallel kinematic chain(PKM) can be defined as closed loop mech-
anism where a base is connected to a mobile platform that has at least two independent
kinematic chains.
Figure 1.2: Parallel manipulator [29]
A parallel manipulator shown in Figure 1.2 is a Stewart platform [28]. The platform
has six degrees of freedom and requires six motors to have the full six degrees of freedom.
The manipulator has more than two independent kinematic chains forming a loop. The
parallel manipulators have many industrial applications like rapid pick and place tasks
using Delta robots [2], driving and flying simulators using Stewart platform [28], etc.
2
Tree-type Robots:
A tree type robot can be defined where a base is connected to ”multiple” end-effector with
series of rigid links and joints.
Figure 1.3: TORO [5] Humanoid robot
The tree-type robot in Figure 1.3 has multiple end-effector connected to a base. The
multiple end-effector in this humanoid robot are its hands, feet, head, and legs. There are
no closed loops in the tree-type system.
A particular topological architecture can be chosen based on the application. Some tasks
can be accomplished using serial robots while others can be done using parallel robots. The
advantages and disadvantages of the serial and parallel robots are summarised in Table 1.1.
Table 1.1: Serial v/s Parallel robots
Serial robots Parallel robots

Easier to control (+) Difficult to control (-)
Large workspace (+) Limited workspace (-)
Low stiffness (-) High stiffness (+)
Low precision (-) High precision (+)
Low payload capacity (-) High payload capacity (+)
Acceleration limits (-) High accelerations (+)
In Table 1.1, the (+) indicates the advantages and (-) indicates the disadvantages of a serial
or parallel robot. It can be seen that serial and parallel robots complement each other.
Series-Parallel Hybrid Robots:

Series-parallel hybrid robots can be defined as a combination of series or tree-type chains
and parallel mechanisms.
Series-parallel hybrid robots combine the advantages of both serial and parallel robots.
3
Many series-parallel hybrid robots can be seen in Figure 1.4. It is quite evident to use
hybrid robots as they can use parallel kinematic chains to have good stiffness and dynamic
characteristic along with serial chains for a larger workspace. An extensive survey on
series-parallel hybrid robots is available in literature [16].
(a) CHARLIE [12] robot (b) MANTIS [1] robot
(c) RECUPERA [19] ex-

oskeleton
Figure 1.4: Series-Parallel hybrid robots
The above Figure 1.4 shows some hybrid robots. One can easily notice the parallel
mechanism of the Stewart platform in the middle of CHARLIE [12] robot and the parallel
mechanism in the leg of the MANTIS [1] robot. In addition to this exoskeletons are also
popular when it comes to series-parallel hybrid robots. RECUPERA exoskeleton [19] can
be seen in the figure. The parallel mechanism can be easily seen as an active ankle [18] in
the foot of the exoskeleton.
4
1.1.2 Complexity in Modeling of Series-Parallel Hybrid Robots
In the previous section, the advantages of series-parallel hybrid robots are discussed. While
there are many advantages of having such hybrid robots, the kinematic complexities are
also inherited from the series and parallel robots. The kinematic complexities arise due to
the presence of closed loops in the hybrid robots. The loop closures in series-parallel hybrid
robots increase with an increase in the number of parallel kinematic chains. Hence, it be-
comes very important to resolve the loop closures in the hybrid robots. Many commercial
software (e.g. ADAMS , RecurDyn, Simmechanics, V-Rep, etc.) and a very limited number
of rigid body dynamics libraries (e.g. RBDL[8], DARTS, OpenSim[26] [4]) provides this
support. Solving the equations of motion for rigid body system with closed loops require
resolution of loop closure constraints which are often solved via numerical procedures. This
brings an additional burden to these algorithms as they have to stabilize and control the
loop closure errors additionally. Also, the numerical solvers lead to inaccuracies, and the
errors increase with an increase in the number of loop closures.
To circumvent this issue, a modular and analytical approach was developed in the form
of Hybrid Robot Dynamics Library (HyRoDyn) [14, 17]. HyRoDyn allows solving the loop
closures in the parallel kinematic chains analytically. It allows the user to model hybrid
robots using the concept of a submechanism module. A hybrid robot can consist of many
submechanisms that can be serial or parallel, and the analytical solutions are provided
in terms of symbolic expressions for a parallel kinematic chain. This is a great tool as it
also allows the user the flexibility to model the robot as a set of submechanism modules
promoting effective reuse of components.
However, the HyRoDyn lacks behind when it comes to any general mechanism for which
the analytical expressions or submechanism library does not exist. In such a case, symbolic
expressions are required for the analyses. Deriving the kinematic equations is not easy for
complex mechanisms with multiple loop closures in a parallel kinematic chain and it can
take users time to implement a new submechanism library.
Both numerical and analytical approaches are used in solving loop closure functions.
The numerical and analytical formulations have their advantages and disadvantages. This
is summarised in Table 1.2. In Table 1.2, the (+) indicates the advantages and (-) indicates
the disadvantages. It can be seen that numerical and analytical formulations complement
each other.
5
Table 1.2: Numerical v/s Analytical formulations
Numerical Approach Analytical Approach

Arbitrary mechanism can be solved nu- Arbitrary mechanisms cannot be solved.
merically without manual code and de- Equations need to be derived for solving
rived equations (+) loop closure mechanisms (-)
Loop closure errors exists (-) Zero errors as the expressions are derived
analytically (+)
Computational efficiency is compromised Computationally fast (+)
(-)
Hence, this motivates a novel approach to combine the numerical and analytical for-
mulations for resolving loop closure equations.
1.2 Objectives
The objectives for a novel hybrid numerical and analytical approach are set, to have an
efficient library for resolving loop closures in closed loop systems.
• Develop a combination of numerical and analytical formulations for resolving loop

closure equations.
• The flexibility resides with the user to choose between a numerical or analytical
formulation.
• Implement in HyRoDyn software that has a modular framework for submechanisms.
• Validate for accuracy and efficiency for resolving the loop closures.
• Test on a real hybrid robot to verify the robustness.
1.3 Methodology
There are many ways to resolve loop closures in a parallel kinematic chain. The famous
approach available in the literature [27, 3, 10, 6] is using a cut-joint approach. In this
approach, a cut joint is introduced in the system that leads to a tree-type system. As
already mentioned, serial or tree-type robots are easy to solve for equations of motion
when compared to parallel robots. The equations of motion can then be constrained using
the constraint forces that arise due to the loop joints.
The equation of motion for a serial or tree-type system can be written as-
τ = M (q)q̈ + C(q, q̇) (1.1)
6
For a robot with loop closures having constraints as φ(q) = 0,
τ + τc = M (q)q̈ + C(q, q̇) (1.2)
where
• τ is the generalised forces vector (forces and torques).
• M (q) is the generalised mass-inertia matrix
• C(q, q̇) is the generalised matrix of Coriolis, centrifugal efforts and gravity efforts.
• q̈ is the vector of generalised acceleration
• q̇ is the vector of generalised velocity
• q is the vector of generalised positions
• τc is the constraint forces due to loop joints.
In literature, the loop closures can be resolved analytically as described in [14, 6]. Also,
the explicit constraints can be obtained from the implicit constraints as described in [10].
This ensures the robustness and accuracy of results as the explicit constraints lead to
accurate results.
1.4 Articulate Body Algorithm with Constraint Embedding

The forward dynamics problem is described as solving for generalized accelerations when
the generalized forces are provided.
q̈ = F Dyn(q̇, q, τ ) (1.3)
−1
q̈ = M (q) (τ + τc − C(q, q̇)) (1.4)
It can be seen that the forward dynamics require the inversion of the mass-inertia matrix.
If the robot has several degrees of freedom, the mass-inertia matrix is quite large. Inverting
such a large matrix is not efficient when there is a requirement for fast forward dynamic
analyses. Other approaches have been discussed in the literature where the forward dy-
namics can be done recursively and leads to O(n) algorithm, where n is the number of
bodies in the system.
In literature, O(n) algorithm from [6] is called Articulated Body Algorithm (ABA).
The work done in [10] gives the idea of embedding the loop closure constraints within the
ABA algorithm. When there is a methodology to resolve the loop closures correctly, it
can be extended to have a forward dynamics algorithm with constraint embedding. The
algorithm is therefore described which can recursively do the forward dynamics. This adds
an extra feature to the HyRoDyn library and not much software gives this flexibility for
solving fast forward dynamics with loop closures.
7
1.5 Contributions
The contributions that are aimed with the thesis are listed below.
• Increase the generality of HyRoDyn software that allow the users to use the software
with confidence even when the analytical solutions for a parallel submechanism are
not available.
• It will save a huge amount of time for the users from providing the analytical solutions.
• Although, analytical approach gives the better computational performance results

than the numerical approach, the hybrid numerical and analytical approach is faster
than solving the full system numerically.
• The constraint embedding approach for fast recursive forward dynamics algorithms.
As there are only a few software like DARTS that uses this approach, more software
can adopt this approach or use the HyRoDyn software.
1.6 Structure
The thesis is organized into a total of seven chapters. The summary of each chapter is
provided below.
Chapter 1: Introduction motivates and sets the objectives of the thesis. It provided
the basic methodology and contribution of the thesis.
Chapter 2: Modeling of Rigid Body System gives the graph based topology to model
rigid body system. The chapter provides a basic introduction to screw theory concepts and
mathematical background.
Chapter 3: Numerical Approach for Resolving Loop Closure Equations shows
the numerical approach for extracting explicit constraints from the implicit constraints.
Chapter 4: Implementation of Hybrid Numerical and Analytical Approach dis-
cusses the implementation of the numerical approach. The chapter presents the modeling
using a hybrid numerical and analytical approach.
Chapter 5: Constraint Embedding for Forward Dynamics presents a recursive for-
ward dynamics algorithm to handle loop closures in the rigid body system.
Chapter 6: Validation and Results checks the validation of the approach and discusses
computational performance.
Chapter 7: Conclusion and Future Work concludes the thesis and discusses the fu-
ture scope of the thesis.
8
Chapter 2
Modeling of Rigid Body systems
This chapter is dedicated to the modeling of series-parallel hybrid robots. The modeling of
rigid body system as presented in [6, 10] is based on graph based topology where the full
rigid body system can be considered as a graph. The topological graph is used to formulate
equations of motion for closed loop systems. For closed loop systems, the equations of
motion can be formulated in implicit and explicit form. For formulating the equations of
motion, screw theory and lie group concepts are used in [23]. These concepts can also
be understood from various textbooks like [24, 6, 20]. The chapter is divided into three
sections, Section 2.1 presents how a rigid body system can be represented as a topological
graph. Section 2.2 gives the background on the screw theory concept that will be used later
in the thesis. Then, Section 2.3 presents the implicit and explicit constraints in closed loop
systems and Section 2.4 discusses the formulation of constraint equations of motion of a
rigid body system. In the end, Section 2.5 presents a simple planar mechanism example
for modeling a rigid body system.
2.1 Graph Based Topology

The equation of motions can be developed using various components of a graph. This is
referred to as the topology of the multi-body system. Given a multi-body system, a graph
based topology can be drawn respecting the following properties,
1. The bodies are represented by nodes.
2. The joints are represented by edges in the graph.
3. The graph should be directed, i.e. edge is directed from parent node to child node.
4. The graph should be connected, i.e. there must exist a path between any two nodes.
5. Exactly one body in the graph represents fixed body referred to as root link or base
in the graph.
9
The graphs can be used to represent any mechanical system like closed loop mechanisms
in the system. When there is no closed loop in the system, the graph becomes a spanning
tree T , i.e. there is only one path between any two nodes in the graph.
2.1.1 Numbering Scheme

A system can be represented by numbering the bodies and joints of the multi-body system.
A convenient numbering scheme is described in [6]. According to the scheme, bodies are
numbered from 0 to NB and joints are numbered from 0 to NJ . By following the steps
below, the nodes and edges can be numbered in the graph.
1. Choose a spanning tree T .
2. The fixed root link is numbered 0, representing the fixed base.
3. The remaining nodes in the graph are numbered from 1 to NB in any order such that
the child node has a higher number in the graph than its parent node.
4. Number the edges from 1 to NB such that the edge i connects between node i and
its parent.
5. Number all the remaining edges from NB + 1 to NJ in any order.
6. Each body gets the same number as its node and each joint gets the same number
as its edge.
As described in [6], this scheme is unique for serial chain systems but it is not unique in
other cases like branched kinematic trees and closed loop systems. A chosen spanning tree
for a closed loop system can be numbered in multiple ways.
2.1.2 Different Properties of a Graph

When the system is represented as a graph, different properties can be derived like,
1. Predecessor array p: It is defined as the array of all predecessor bodies. p(i) is the
predecessor of joint i.
2. Successor array s: It is defined as the array of all successor bodies. s(i) is the
successor of joint i.
3. Parent array λ: It is defined as the parent of each body in the spanning tree. λ(i) is
the parent of body i.
4. Child array µ: It is defined as the set of children of body i including the base.
5. Support array κ: For any body i except base, κ(i) is the set of all tree joints between
body i and base.
10
2.2 Screw Theory and Lie Group Methods
This section discusses the modeling of kinematics using the screw theory. The content is
inspired from [20, 23, 6, 22]. The kinematic formulations can be performed using various
representations like spatial representation, body-fixed representation, hybrid representa-
tion given in [23]. This section gives a brief introduction to the kinematic formulations of
the serial chains using the spatial and body-fixed representation of twists from [22, 23].
The screw theory concepts are inspired by the motion of a screw, where the screw is
rotating and translating along the same fixed axis. It gives a six parameters representation
which are called the exponential coordinates [20]. A screw can be defined as

ê
S= (2.1)
x × ê + hê
where ê is the unit vector along the screw joint axis, x is the position vector to any point
on this axis, and h is the pitch.
The lower pair joints with one degree of freedom can be written in terms of screw coor-
dinates. A screw motion between two interconnected bodies with a lower pair joint has
a certain pitch h. Revolute joints have zero pitch, h = 0, and prismatic joints have an
infinite pitch, h = ∞. Let S denote the unit screw axis, then

ê 0
S revolute = S prismatic = (2.2)
x × ê ê
where ê is the unit vector along the joint axis in the joint frame.
Notation: In this thesis, the bold letters denote vector and matrices. When there is
a chance of ambiguity, [] denotes the matrix form of the variable. For example, a screw
can be represented as S = [ω, v]T ∈ R6 , can be represented as an element of se(3) as

[ω] v
[S] = (2.3)
0 0
Matrix Exponential: Let S = (ω, v) gives the screw coordinates. The matrix exponen-
tial is defined as mapping from exp : [S]θ ∈ se(3) → T ∈ SE(3).
If ||ω|| = 1, then for any distance q ∈ R along the screw axis or any angle θ ∈ R rotated
about the axis,
R (Iθ + (1 − cos θ)[ω] + (θ − sin θ)[ω]2 )v

T = exp[S]θ = ∈ SE(3) (2.4)
0 0
where,
R = exp[ω]θ = I + sin θ[ω] + (1 − cos θ)[ω]2 ∈ SO(3) (2.5)
11
and  
0 −ωz ωy
[ω] =  ωz 0 −ωx  (2.6)
−ωy ωx 0
If ||ω|| = 0 and ||v|| = 1, then

I vθ
T = exp[S]θ = ∈ SE(3) (2.7)
0 0
2.2.1 Position Analysis

The position analysis is the forward kinematics of the kinematic chain that can be used
to compute the position of the end-effector from the base. It can be computed recursively
using the product of exponential (POE) formula.
In spatial representation
Product of exponential: Given the reference configuration of the end-effector 0 T n (0) ∈
SE(3), forward kinematics can be defined as
0
T n (q) = exp([S 1 ]q1 ) exp([S 2 ]q2 ) . . . exp([S n ]qn )0 T n (0) (2.8)
where S i is the screw coordinates of the ith
joint expressed in base frame.
The configuration of body i is denoted by T i ∈ SE(3) which defines the frame transforma-
tion from the body-fixed frame to the inertial frame. For ground link T 0 = I. The relative
configuration between body j with respect to body i is defined as
i
T j = T i,j = T −1
i Tj (2.9)
Ri,j i r i,j

T i,j = (2.10)
0 1
where Ri,j ∈ SO(3) is the rotation matrix and i r i,j ∈ R3 is the position vector.
In body-fixed representation
The product of exponential formula can be rewritten as
T i = B 1 exp([1 X 1 ]q1 )B 2 exp([2 X 2 ]q2 ) . . . B i exp([i X i ]qi ) (2.11)
where
• B i := T i−1,i (0) is the reference configuration of body frame i relative to its parent
i − 1.
• i X i is the screw coordinates of the vector of joint frame i represented in body-fixed
frame i. Notice that i X i is constant.
12
2.2.2 Velocity Analysis
The twist of a body is denoted by V b = (ω b , v b )T in body-fixed representation. In the
spatial representation, the twist of a body is denoted by V s = (ω s , v s )T . The superscript
b is used for body-fixed representation and superscript s is used for spatial representation.
Note: If the superscript is not mentioned, it should be assumed to have the body fixed
representation.
In spatial representation, the recursive velocity equation can be written as
V si = V si−1 + AdTi,0 S i q̇i (2.12)
where S i is the screw coordinates of the ith joint expressed in base frame.
The twists of all bodies can be summarised in V ∈ R6n as
V s = J s q̇ (2.13)
The system level Jacobian matrix J s ∈ R6n×n can be written in terms of factorization
J s = As S s (2.14)
where
   
AdT1 0 0 ... 0 S1 0 0 ... 0
AdT AdT2 0 ... 0   0 S2 0 ... 0 
 1   
s AdT AdT2 AdT3 ... 0  s 0 0 S3 ... 0
A =  ,S =   (2.15)

1
 .. .. .. .. ..  .. .. .. .. .. 
 . . . . .  . . . . . 
AdT1 AdT2 ... AdTn−1 AdTn 0 0 ... 0 Sn
The velocity of body i can be defined recursively as -
V i = AdTi,i−1 V i−1 + i X i q̇i (2.16)
where q̇i is the velocity of joint i and AdTi is the adjoint transformation matrix of size
6 × 6 defined as-
Ri,j 0
AdTi,j = i (2.17)
[ r i,j ]Ri,j Ri,j
The twists of all bodies can be summarised in V ∈ R6n as
V = J q̇ (2.18)
13
The system level Jacobian matrix J ∈ R6n×n can be written in terms of factorization
J = AX (2.19)
where
  1 
I 0 0 ... 0 X1 0 0 ... 0
 AdT I 0 ... 0  0 2X 0 ... 0
2

 2,1   
 AdT AdT3,2 I ... 0 3X
A=  ,X =  0 0 3 ... 0
 
3,1 
 .. .. .. .. ..   .. .. .. .. .. 
 . . . . .  . . . . . 
AdTn,1 AdTn,2 ... AdTn,n−1 I 0 0 ... 0 nX
n
(2.20)
2.2.3 Acceleration Analysis

To acceleration can be found by differentiating the equation V = J q̇,
V̇ = J q̈ + J˙q̇ (2.21)
In spatial representation, the acceleration equation is given recursively by differentiating
Equation 2.12
s s
V̇ i = V̇ i−1 + AdTi,0 S i q̈i + [V i−1 , V i ] (2.22)
Here, [a, b] represents the spatial cross product between a and b or also known as the Lie
brackets.
The adV is defined as
[ω] 0
adV = (2.23)
[v] [ω]
s s
V̇ i = V̇ i−1 + AdTi,0 S i q̈i + adV i−1 V i (2.24)
Recursively, the acceleration of body i can be found by differentiating Equation 2.16,
˙ T
V̇ i = AdTi,i−1 V̇ i−1 + Ad i
i,i−1 V i−1 + X i q̈i (2.25)
The time derivative of adjoint transformation yields [21],

˙ T
Ad i
i,i−1 V i−1 = [AdTi,i−1 V i−1 , X i q̇i ] (2.26)
Hence,
V̇ i = AdTi,i−1 V̇ i−1 + [AdTi,i−1 V i−1 , i X i q̇i ] + i X i q̈i (2.27)
14
V̇ i = AdTi,i−1 V̇ i−1 + [V i , i X i q̇i ] + i X i q̈i (2.28)
V̇ i = AdTi,i−1 V̇ i−1 − [i X i q̇i , V i ] + i X i q̈i (2.29)
V̇ i = AdTi,i−1 V̇ i−1 − adi X i q̇i V i + i X i q̈i (2.30)
This concludes the screw theory and lie group concepts that are required for the purpose
of this thesis.
2.3 Mathematical Background on Constraint Equations of

Motion
The constraints in the rigid body system can be widely seen in terms of contact and loop
closure constraints. This section aims to give a brief idea on the loop closure constraints.
The formulation of equations of motion for the loop closures can be done using implicit
and explicit constraints using cut joint approach. The equations of motion shall be written
for a tree-type system and has to be constrained using the constraint forces due to the cut
joint. The procedure can be summarised as
1. Formulate equations of motion for the tree-type system of the closed loop system.
2. The addition of constraint forces due to the loop closures in the equation.
3. Formulation of kinematic constraint equations due to the loop closure.
4. Combining the two formulated equations.
The tree-type system has n degrees of freedom along with nc constraints. The variables
can be defined as
NB
X NJ
X
n= ni and nc = nck (2.31)
i=1 k=NB +1
In the above equation, ni is the degrees of freedom of the tree joint i, and nck is the number
of constraints due to the cut joint k in the loop.
2.3.1 Implicit Constraints

Consider a rigid body system where p(k) and s(k) defines the predecessor and successor
bodies of the cut joint k. The POE equation can be written as
 
Y
0
T p(k) (q) =  exp ([S i ]qi ) 0 T p(k) (0) (2.32)
i∈κ(p(k))
15
 
Y
0
T s(k) (q) =  exp ([S i ]qi ) 0 T s(k) (0) (2.33)
i∈κ(s(k))
The relative motion between the predecessor and successor bodies is given by: T −1 p(k) T s(k) .
If S k denotes the screw coordinates of the cut joint k, the loop closure condition is given
by
exp ([S k ]qk ) = (p(k) T −1 0 −1 0
k )( T p(k) )( T s(k) )(
s(k)
T k ) ∈ SE(3) (2.34)

∆Rk ∆pk
exp ([S k ]qk ) = (2.35)
0 1
Due to constraints imposed by the cut joint, some components in the above equation should
be zero. For example, if the cut joint is a spherical joint, the position part should be zero.
This defines the implicit constraints and is given by
φ(q) = 0 (2.36)
where q is the vector of n generalized positions.

The velocity implicit constraints can be obtained by differentiating the above equation,
K q̇ = 0 (2.37)
where,
∂φ
K= (2.38)
∂q
Here, K is an nc × n matrix and is called the constraint Jacobian matrix in implicit form.
The acceleration level constraints can be obtained by differentiating the above equation as,
K q̈ + K̇ q̇ = 0 or K q̈ = k (2.39)
where k = −K̇ q̇ is a vector of nc variables and is called as the bias acceleration in implicit
form.
2.3.2 Explicit Constraints

In explicit constraints, a vector of independent joint variables is considered as y. y is
typically a subset of position vector q, but need to be chosen in every case. The independent
joints must be chosen in such a way that y defines q in a unique way. Let y be a vector of
size m.
On position level, based on the above information, a loop closure function γ exists. The
function satisfies the equation
q = γ(y) (2.40)
16
Differentiating the above equation, the velocity level explicit constraint equation can be
written as
q̇ = Gẏ (2.41)
where
∂γ
G= (2.42)
∂y
G is matrix of size n × m and is called constrained Jacobian matrix in explicit form. Now,
differentiate the above equation to formulate the acceleration level constraint in explicit
form,
q̈ = Gÿ + g (2.43)
where
g = Ġẏ (2.44)
g is a vector of n variables and is called bias acceleration in explicit form.
2.3.3 Relationship between the Implicit and Explicit Form

The implicit and explicit form is a way to represent the constraints in the closed loops. If
they describe the same constraint then the reciprocal product between them is zero.
φoγ = 0 (2.45)
Also, φ(q) = 0,
φ(γ(y)) = 0 (2.46)
∂φ ∂γ(y)
=0 (2.47)
∂γ(y) ∂y
As the constraints are same,
∂φ ∂γ(y)
=0 (2.48)
∂q ∂y
From the definition of explicit and implicit Jacobin matrices K and G,
KG = 0 (2.49)
Differentiating the above equation,
K Ġ + K̇G = 0 (2.50)
Multiplying both sides by ẏ,
K Ġẏ + K̇Gẏ = 0 (2.51)
From previous section, g = Ġẏ and q̇ = Gẏ. Hence,
Kg + K̇ q̇ = 0 (2.52)
Kg = k (2.53)
17
2.4 Constraint Equations of Motion in Implicit and Explicit
Form
The equation of motion for tree-type system can be written as
τ = M q̈ + C (2.54)
When the closed loop are appear in the system, the above equation of motion have extra
term that accounts for the constraint forces τc due to the loop closures.
τ + τc = M q̈ + C and K q̈ = k (2.55)
Now, the constraint forces due to the loop closure, τc can be expressed as
τc = K T λ (2.56)
where λ is a vector of nc unknown constraint force variable, also referred to as Lagrangian

multipliers.
Implicit form: Combining the Equations 2.55, 2.39, and 2.56, the equations of motion
for the closed loop system can be expressed as:
M KT

q̈ τ −C
= (2.57)
K 0 −λ k
Explicit form: Using the property of KG = 0, Lagrangian multipliers can be eliminated

as
GT (τ + K T λ) = GT (M q̈ + C) (2.58)
GT τ = GT (M q̈ + C) (2.59)
Substituting q̈ = Gÿ + g into above equation,
GT M Gÿ + GT (C + M g) = GT τ (2.60)
The final equations of motion in independent coordinates(explicit form) can be written as
M y ÿ + C y = τ y (2.61)
where
• τ y = GT τ is the generalised forces (forces and torques).
• M y = GT M G is the generalised mass-inertia matrix (symmetric and positive defi-

nite)
18
• C y = GT (C + M g) is the generalised matrix of Coriolis-centrifugal efforts and
gravity forces.
After the equations of motion are formulated, inverse dynamics and forward dynamics can
be done.
Inverse Dynamics: It can be defined as the computation of generalized forces given the
generalized position y, generalized velocity ẏ, and generalized accelerations ÿ.
τ = IDyn(y, ẏ, ÿ) (2.62)
Forward Dynamics: It can defined as the computation of generalized accelerations given

the generalized forces.
ÿ = F Dyn(y, ẏ, τ ) (2.63)
This concludes the mathematical approach for formulating equations of motion in both
independent coordinates and spanning tree coordinates.
2.5 Example
Consider a planar lambda mechanism, a variant of slider crank mechanism.
Jr1,3
B3
q3
g
B1
Jp2,3
2
L
m1 q3
L2
2
2
q1 q2
B2
Jr0,1
L1 Jr0,2
Figure 2.1: Lambda or RRPR mechanism
Referring to Figure 2.1, the lambda mechanism consist of base of length L0 , link B2
as the stator part of the prismatic actuator, and links B1 and B3 of lengths L2 and q3
r = 1 (revolute joint between base link and
respectively. The joints are described as J0,1
r = 2 (revolute joint between base link and body B ), J r = 4 revolute
body B1 ), J0,2 2 1,3
p
joint between body B1 and body B3 , and J2,3 = 3 (revolute joint between body B1 and
19
prismatic link B3 ).
r = 4 define the spanning tree as- T = {0, 1, 2, 3}.
The introduction of cut joint at J1,3
1
Jr0,1 Jr1,3
1 4
3
2 3
Jr0,2 Jp2,3
2
Figure 2.2: Topological graph for lambda mechanism
The topological graph can be depicted in Figure 2.2, where dashed line represents the
cut joint in the graph. However, there exists other possible ways to draw the topological
graphs by choosing different cut joints. Figure 2.3 presents different possible topological
graphs if other joints were considered as cut joint.
Figure 2.3: Other possible topological graphs for planar lambda mechanism, adapted from
[6]
For the topological graph in Figure 2.2, the arrays can be defined as
• Predecessor array p = {0, 0, 2}
• Successor array s = {1, 2, 3}
• Parent array λ = {0, 0, 2}
• Child array µ = {{1, 2}, {}, {3}, {}}
• Support array κ = {{1}, {2}, {2, 3}}
20
2.5.1 Vector based Loop Closure Analysis
The loop closure equations can be written as below with reference to Figure 2.1.
L2 − L1 − q3 = 0 (2.64)
The x - component of the equation can be written as
L2 cos q1 − L1 − q3 cos q2 = 0 (2.65)
and y-component as
L2 sin q1 − q3 sin q2 = 0 (2.66)
The implicit constraint is therefore,

L2 cos q1 − L1 − q3 cos q2 0
= i.e., φ(q) = 0 (2.67)
L2 sin q1 − q3 sin q2 0
where q = (q1 , q2 , q3 )T .
The matrix K = ∂φ ∂q , can be found by differentiating equation
 
q˙
−L2 sin q1 q3 sin q2 − cos q2  1 

0
q˙2 = (2.68)
L2 cos q1 −q3 cos q2 − sin q2 0
q˙3
K q̇ = 0 (2.69)
Hence, matrix K is
−L2 sin q1 q3 sin q2 − cos q2
K= (2.70)
L2 cos q1 −q3 cos q2 − sin q2
Now, acceleration implicit constraint equation can be achieved by differentiating above
equations.
L2 cos (q1 ) q̇21 − q3 cos (q2 ) q̇22 − 2 q̇3 sin (q2 ) q̇2

k = −K̇ q̇ = (2.71)
L2 sin (q1 ) q̇21 − q3 sin (q2 ) q̇22 + 2 q̇3 cos (q2 ) q̇2
2.5.2 Analysis using Screw Theory

Considering the RRPR mechanism, we have NB = 3, and NJ = 4. The number of
r .
kinematic loops is NJ − NB = 1. The cut joint as defined in Figure 2.2 is k = 4 = J1,3
The screw coordinates of the above model can be written as
21
Table 2.1: Screw coordinates for different joints

ê
Joint Pitch h x ê S=
x × ê + hê
T T T
Joint 1 0 0, 0, 0 0, 0, 1 0, 0, 1, 0, 0, 0
T T T
Joint 2 0 L1 , 0, 0 0, 0, 1 0, 0, 1, 0, −L1 , 0
T T T
Joint 3 ∞ 0, 0, 0 1, 0, 0 0, 0, 0, 1, 0, 0
In Table 2.1, x is the vector from the inertial frame to the body-fixed frame in zero
configuration. ê is the unit vector along the direction of joint axis. From Figure 2.1, the
revolute joint axis for joint 1 and 2 is in the positive z direction, and the prismatic joint
axis for joint 3 is along the positive x axis.
2.5.3 Implicit Form

Position level implicit constraint:
Using screw theory, the position level loop constraint can be defined for the two serial link
chains. For the predecessor body of the joint 4, p(k) = 1,
0
T p(k) (q) = exp ([S1 ]q1 )0 T p(k) (0) (2.72)
where 0 T p(k) (0) = I 4×4 is zero pose configuration of the predecessor body.
 
cos (q1 ) − sin (q1 ) 0 0
0
 sin (q1 ) cos (q1 ) 0 0 
T p(k) (q) =   (2.73)
 0 0 1 0 
0 0 0 1
For the successor body of the joint 4, s(k) = 3,

0
T s(k) (q) = exp ([S2 ]q2 ) exp ([S3 ]q3 )0 T s(k) (0) (2.74)
where 0 T s(k) (0) is zero pose configuration of the successor body given by
 
1 0 0 L1
0
0 1 0 0
T s(k) (0) = 
0
 (2.75)
0 1 0
0 0 0 0
 
cos (q2 ) − sin (q2 ) 0 L1 + q3 cos (q2 )
0
 sin (q2 ) cos (q2 ) 0 q3 sin (q2 ) 
T s(k) (q) =   (2.76)
 0 0 1 0 
0 0 0 1
22
The relative motion of the between the predecessor and successor body is given by T −1
p(k) T s(k) .
The loop closure condition can be written as
exp ([S k ]qk ) = (p(k) T −1 0 −1 0
k )( T p(k) )( T s(k) )(
s(k)
T k) (2.77)

∆Rk ∆pk
exp ([S k ]qk ) = (2.78)
0 1
 
1 0 0 L1 − L2 cos (q1 ) + q3 cos (q2 )
 0 1 0 q3 sin (q2 ) − L2 sin (q1 ) 
exp ([S k ]qk ) = 
 0 0 1
 (2.79)
0 
0 0 0 1
As the cut joint is a revolute joint, the position part of the matrix should be zero.
∆pk = 0 (2.80)
This will define our implicit constraints of the model.
 
L1 − L2 cos (q1 ) + q3 cos (q2 )
φ(q) =  q3 sin (q2 ) − L2 sin (q1 ) =0 (2.81)
0
Velocity level implicit constraint:
The velocity level loop constraints can be written as
AdT −1 AdT −1 (V ss(k) − V sp(k) ) = S k q̇k (2.82)
p(k),k 0,p(k)
Multiplying by the orthogonal complement of S k , i.e. W Tk , as W Tk S k = 0,

W Tk AdT −1 AdT −1 (V ss(k) − V sp(k) ) = 0 (2.83)
p(k),k 0,p(k)
Rewriting the above equation,

W Tk AdT −1 AdT −1 (J ss(k) − J sp(k) )q̇ = 0 (2.84)
p(k),k 0,p(k)
where J si is the spatial jacobian matrix of body i.

Therefore, constraint jacobian matrix can be written as
K = W Tk AdT −1 AdT −1 (J ss(k) − J sp(k) ) (2.85)
p(k),k 0,p(k)
Computing the above expression,

−L2 sin (q1 ) q3 sin (q2 ) − cos (q2 )
K= (2.86)
L2 cos (q1 ) −q3 cos (q2 ) − sin (q2 )
which is the constraint jacobian matrix.
The acceleration level implicit constraints can be obtained by differentiating the velocity
level constraint equation.
23
2.5.4 Explicit Form
In explicit form, a subset of position vector need to be chosen. The independent coordinate
selected in this case is q1 ,
y = q1 (2.87)
Position level explicit constraints:
For explicit constraints, a function γ is defined to satisfy q = γ(y). From Equation 2.65
and 2.66, q2 and q3 is written as a function of q1 .
L2 sin q1
q2 = tan−1 (2.88)
L2 cos q1 − L1
q
q3 = L21 + L22 − 2L1 L2 cos q1 (2.89)
Therefore,  
q1
q=
 tan−1 L2Lcos
2 sin q1
q1 −L1

 (2.90)
p
2 2
L1 + L2 − 2L1 L2 cos q1
∂γ
G matrix is defined as G = ∂y . Differentiating the above equations w.r.t. y or q1 ,
 
1
 L2 −L1 L2 cos q1 
G =  2 q2  (2.91)
3
L1 L2 sin q1
q3
Differentiating q̇ = Gẏ again,

q̈ = Gÿ + g (2.92)
where
0
 
L2 q̇1 (2 L1 q̇3 cos(q1 )−2 L2 q̇3 +L1 q̇1 q3 sin(q1 ))
g = Ġẏ =  (2.93)
 
q33 
L1 L2 q̇1 (q̇3 sin(q1 )−q̇1 q3 cos(q1 ))
− q32
2.5.5 Equivalence of Implicit and Explicit Constraints

The next step is to check if implicit and explicit constraints are same. To verify this,
KG = 0.  
1
−L2 sin q1 q3 sin q2 − cos q2  L22 −L1 L2 cos q1 
KG = q32
 
L L sin q 1 2 1
q3
24
Considering only first row,
q3 sin q2 L22 − L1 L2 q3 cos q1 sin q2 L1 L2 sin q1
KG1 = −L2 sin q1 + 2 − cos q2 (2.94)
q3 q3
L2 sin q1 L22 − L1 L2 q3 cos q1 sin q2 (L2 cos q1 − L1 ) L22 sin q1
KG1 = −L2 sin q1 + −
q32 q3 q3
2 2 2
L2 sin q1 L2 − 2L1 L2 cos q1 sin q1 + L1 L2 sin q1
= −L2 sin q1 + (2.95)
q32
L2 − 2L1 L2 cos q1 + L21
= −L2 sin q1 + L2 sin q1 22 =0
L1 + L22 − 2L1 L2 cos q1
Similarly, for second row of KG,
L22 − L1 L2 cos q1 L1 L2 sin q1
KG2 = L2 cos q1 − q3 cos q2 2 − sin q2
q3 q3
2
L − L1 L2 cos q1 L2 sin q1 L1 L2 sin q1
= L2 cos q1 − (L2 cos q1 − L1 ) 2 −
q32 q3 q3
(L3 cos q1 − L1 L22 cos2 q1 − L1 L22 + L21 L2 cos q1 ) L1 L22 − L1 L22 cos2 q1
= L2 cos q1 − 2 −
q32 q32
(L2 − L1 L2 cos q1 + L21 − L1 L2 cos q1 )
= L2 cos q1 − L2 cos q1 2 =0
q32
(2.96)
Hence, as KG = 0, the implicit and explicit constraints are the same.
2.5.6 Constraint Equations of Motion in Implicit and Explicit Form

The equations of motion for a tree-type system.
τ = M q̈ + C (2.97)
For the lambda mechanism, generalized mass inertia matrix can be computes using La-
grangian dynamics as  
I1 0 0
M =  0 m3 q3 2 + I2 + I3 0  (2.98)
0 0 m3
where m3 is the mass of link B3 , and I1 , I2 , I3 is the joint space inertia components.
The generalised matrix of Coriolis-centrifugal efforts and gravity forces can be computed
as  L2 g m1 cos(q1 ) 
2
C= 2 q̇2 q̇3 m3 q3 + g m3 q3 cos (q2 )  (2.99)
−m3 q3 q̇22 + g m3 sin (q2 )
25
where g is the gravity term, and m1 is the mass of link B1 .
Hence, the equation of motion is given by
I1 q̈1 + L2 g m12 cos(q1 )

 
τ =  q̈2 m3 q3 2 + I2 + I3 + 2 q̇2 q̇3 m3 q3 + g m3 q3 cos (q2 )  (2.100)

−m3 q3 q̇22 + q̈3 m3 + g m3 sin (q2 )
The constraint forces can be computed as
τ c = KT λ (2.101)
In implicit form, the constraint equations of motion can be written using Equation 2.57.
The unknown Lagrangian multipliers can be eliminated using the property KG = 0, and
the explicit form of the constraint equation of motion is a single equation in independent
coordinates,
τy = My ÿ + Cy (2.102)
After symbolic computation, the expression for τy is derived as,
!
L2 2 (L2 − L1 cos (q1 ))2 m3 q3 2 + I2 + I3 L1 2 L2 2 m3 sin (q1 )2

τy = q̈1 I1 + +
q3 4 q3 2
L2 g m1 cos (q1 )
+
2
L2 (L2 − L1 cos (q1 )) (2 q̇2 q̇3 m3 q3 + g m3 q3 cos (q2 ))
+
q3 2
L2 (L2 − L1 cos (q1 )) L2 q̇1 m3 q3 2 + I2 + I3 (2 L1 q̇3 cos (q1 ) − 2 L2 q̇3 + L1 q̇1 q3 sin (q1 ))

+
q3 5
L1 L2 m3 sin (q1 ) −L1 L2 cos (q1 ) q̇1 q3 + L1 L2 q̇3 sin (q1 ) q̇1 + q̇22 q3 3 − g sin (q2 ) q3 2
2

−
q3 3
(2.103)
This completes the constraint equations of motion for the closed loop system.
26
Chapter 3
Numerical Approach for Resolving

Loop Closure Equations
This chapter is dedicated to the formulation of explicit loop closure constraints from a
numerical approach. In most of the numerical approaches, the aim is to find the implicit
constraints as they are fairly easy to obtain using recursive formulations like the Newton-
Rhapson method. As the loop closures in the system increases, it is difficult to control
loop closure errors. To control the violation of constraints, stabilization parameters like
Baumgarte stabilization parameters are provided [8]. But, it does not ensure that the loop
closure errors are zero. Due to inaccuracies, these methods are difficult to implement in
the control strategies of a system. The control architecture has to handle extra errors due
to modeling. On the other hand, explicit constraints are complex but the loop closures
errors are zero. For explicit constraints, [14] presents an analytical approach where the
symbolic expressions are provided as a library to resolve loop closure errors. As discussed
in the previous chapter, if the implicit and explicit forms represent the same constraints,
one form can be obtained from the other [6, 10]. This chapter is divided into sections where
Section 3.1 gives the introduction to existing algorithm from Rigid body Dynamics Library
(RBDL)[8], Section 3.2 presents the mathematical formulations for extraction of explicit
constraints from the implicit constraints, and last Section 3.3 presents the example.
3.1 Software Dependency on Rigid Body Dynamics Library

The Rigid Body Dynamics Library (RBDL) [8] is based on the formulations in [6]. RBDL is
an open-source library that used recursive methods in reduced or generalized coordinates
for formulations of rigid-body dynamics. RBDL provides the support for loop closure
constraints but it deals with implicit constraints. The important functions required from
RBDL for our approach are mentioned here.
1. AddLoopConstraint(): Add loop constraints to the constraint set. The loop con-
27
straints can be described for a closed loop system. The RBDL provides an extension
to LUA [9] model file where the constraint set can be defined. For a cut joint, the
constraints are added with information of predecessor body id, successor body id, a
relative transformation between the cut joint and predecessor body, a relative trans-
formation between the cut joint and successor body, constraint axis, and Baumgarte
stabilization parameter. The loop closure constraints should be provided by elimi-
nating the redundant constraints for the constraint axes.
2. CalcConstraintsJacobian(): Gives the constrained jacobian matrix in implicit form.

The function expects the inputs as the model of the system, e.g. LUA model, the gen-
eralized position of the joints, and constraint set defined using the function AddLoop-
Constraint(). From the previous chapter,
K = W Tk AdT −1 AdT −1 (J ss(k) − J sp(k) ) (3.1)

p(k),k 0,p(k)
The function implements the above equation in the function CalcConstraintsJaco-

bian().
3. CalcAssemblyQ(): Gives the explicit constraint function γ. This function requires

an initial guess of the generalized positions, weighting coefficients for the joints,
constraint set, and the model. The function solves the linear equation of the form
Ax = b using the iterative Newton-Rhapson method. It provides the provision to set
weighting coefficient to the joints that can be used to set independent coordinates y
for explicit form. The weights of the independent coordinates are set to 1, maintaining
that independent coordinates do not change in the iterative computation. The weight
coefficient of all other joints can be set to zero. This ensures that generalized positions
can be found as a function of the independent coordinates, hence the explicit form
q = γ(y).
3.2 Mathematical Formulations

3.2.1 Extraction of Explicit Jacobian Matrix from Implicit Jacobian Ma-
trix
Using RBDL function CalcConstraintsJacobian(), the constraint jacobian matrix in implicit
form K is computed. Matrix K can be rewritten by splitting it into independent and
dependent coordinates part. Dependent coordinates is a subset of generalized positions
that are expressed as a function of independent coordinates y = q i ∈ Rm using function γ.
K q̇ = 0 (3.2)

q̇ i
Ki Kd =0 (3.3)
q̇ d
28
where
• K i is the independent coordinates part of matrix K. The size of the matrix is

nc × m matrix, where nc is number of constraints due to the cut joint in the closed
loop system.
• K d is the dependent coordinates part of matrix K. The size of the matrix is nc ×

(n − m) matrix. Note that it is a square matrix.
• q̇ i = ẏ is the generalized velocity vector of size (m) in independent coordinates.
• q̇ d is the generalized velocity vector of size (n − m) in dependent coordinates.
Expanding the previous equation,
K i ẏ + K d q̇ d = 0 (3.4)
q̇ d = −K −1
d K i ẏ (3.5)
Or, in matrix form,
q̇ i I
q̇ = = ẏ (3.6)
q̇ d −K −1
d Ki
Also, q̇ = Gẏ. Therefore, the constraint jacobian matrix in explicit matrix can be written
as

I
G= (3.7)
−K −1
d Ki
To split the matrix K into independent and dependent parts, selection matrices Qi and
Qd are defined respectively. The independent joints selection matrix Qi is of size (m × n)
and dependent joints selection matrix Qd is of size ((n − m) × n).
The algorithm to extract G from K is given in Algorithm 1.
Algorithm 1 calc G from K(K)

Require: K
Ensure: G
1: K i = KQT i
2: K d = KQT d
−1
3: Gd = K d K i
4: Gi =
I
Gi
5: G ←
Gd
29
3.2.2 Extraction of Explicit Bias Acceleration from Implicit Bias Accel-
eration
This section presents the extraction of explicit bias acceleration vector g from implicit bias
acceleration vector k. For bias accelerations in implicit form k, the RBDL function Calc-
ConstrainedSystemVariables() is used. To compute only the bias acceleration the function
is rewritten as calc k(). The function take the input as model, generalized positions, gen-
eralized velocities and constraint set. It return the bias acceleration in implicit form k.
Differentiating the velocity equation K q̇ = 0
K q̈ + K̇ q̇ = 0 (3.8)
The bias acceleration in implicit form is given by
k = −K̇ q̇ (3.9)
As in the previous section, the equation can be rewritten in terms of dependent and inde-
pendent parts.
q̈ i
Ki Kd = −K̇ q̇ (3.10)
q̈ d
K i ÿ + K d q̈ d = −K̇ q̇ (3.11)
Multiplying by K −1
d ,
K −1 −1
d K i ÿ + q̈ d = −K d K̇ q̇ (3.12)
q̈ d = −K −1 −1
d k − K d K i ÿ (3.13)
Now, as q̈ = Gÿ + g, substituting G from Equation 3.7 and comparing with the previous
equation,
0
g= (3.14)
Kd−1 k
The algorithm to extract g from k is given in Algorithm 2.
Algorithm 2 calc g from k(K,k)

Require: K, k
Ensure: g
1: K d = KQT d
−1
2: g d = K d k
3: g i = 0

gi
4: g ←
gd
30
3.3 Example of Lambda Mechanism
Continuing the example of planar lambda mechanism from previous chapter, matrix K is
given by
−L2 sin (q1 ) q3 sin (q2 ) − cos (q2 )
K= (3.15)
L2 cos (q1 ) −q3 cos (q2 ) − sin (q2 )
The constraint jacobian matrix split in independent and dependent parts can be written
as
−L2 sin q1 q3 sin q2 − cos q2
Ki = and K d = (3.16)
 
1
I 1 −q2 )
G= =  L2 cos(q (3.17)
 
−K −1 K q3 
d i
−L2 sin(q1 − q2 )
 
1
L2 (cos q1 cos q2 +sin q1 sin q2 )
G= (3.18)
 
q3 
−L2 (sin q1 cos q2 − cos q1 sin q2 )
Substituting from Equation 2.65 and 2.66,
 
1
 L2 (cos q1 (L2 cos q1 −L1 )+sin q1 L2 sin q1 
G= q32  (3.19)
sin q1 (L2 cos q1 −L1 )−cos q1 L2 sin q1
−L2 q3
Simplifying,  
1
 L2 −L1 L2 cos q1 
G =  2 q2  (3.20)
3
L1 L2 sin q1
q3
The above equation is the same as Equation 2.91. Hence, this method demonstrates how
explicit constraint jacobian matrix is extracted from implicit jacobian matrix.
Similarly, bias acceleration in explicit form can be obtained from implicit bias acceleration
using Equation 3.14.
Addition of Loop Constraints

Referring to the topological graph RRPR or lambda mechanism (provided again for easier
access),
31
1
Jr0,1 Jr1,3
1 4
3
2 3
Jr0,2 Jp2,3
2
Figure 3.1: Topological graph of RRPR
In Figure 3.1, the cut joint is k = 4. The predecessor body id in the figure is p(4) = 1,
and the successor body id of the cut joint is s(4) = 3. The relative transformation between
p(k) and k is T p(k),k and between s(k) and k is T s(k),k . Nest the constraint axis should be
defined. For the constraint axis, the joint definition must be considered. For example, if
the cut joint frame axis is in a positive y direction in the local frame, the prismatic joint
puts constraint for the loop closure in the y − z plane.
The constraint axis are defined in spatial form as
T
X cy = 0 0 0 0 1 0

(3.21)
T
X cz = 0 0 0 0 0 1

(3.22)
where X cy is the constraint axis in y direction, and X cz is the constraint axis in z direction.
Note that redundant constraints are not provided while defining constraint axes.
For the LUA model of the RRPR mechanism, refer to Appendix A. It presents the imple-
mentation of constraint sets in the LUA model file.
32
Chapter 4
Implementation of Hybrid
Numerical and Analytical
Approach
The objective of this chapter is to present a novel hybrid numerical and analytical approach
to resolve loop closure equations in multi-body systems. The numerical approach to extract
the explicit constraints is discussed in the previous chapter. In the literature, [14] imple-
ments the explicit constraints for various closed loop systems. The symbolic expressions
of the explicit constraints are generated and the expressions are converted into computer
code. Hybrid Robot Dynamics (HyRoDyn) is such software where the equations of motion
are formulated in explicit form. The symbolically generated C codes are provided for the
loop closures and given as a parallel submechanism module. The software is unique due to
modularity where the multi-body system can be constructed using submechanism modules.
A submechanism module can be a serial chain or a parallel kinematic chain. Also, certain
abstractions are provided in HyRoDyn that add to its uniqueness. The abstractions allow
the modeling and control of series-parallel hybrid robots in joint space, actuation space,
and task space.
However, the software allows the modeling and control of closed loop systems that are
known to its database as parallel submechanism modules. This limitation makes the soft-
ware less general when it comes to any closed loop system. The generation of symbolic
codes for a new parallel kinematic chain is not easy to obtain and usually takes a consider-
able amount of time. The limitation of the software can be overcome by providing explicit
constraints using a numerical approach for closed loop systems.
Hence, a novel approach to combine the numerical and analytical approach is explored in
this chapter and implemented in HyRoDyn software. The chapter is divided into sections
33
where Section 4.1 gives the idea of having a hybrid version of numerical and analytical
formulations, Section 4.2 presents further development in the user interface architecture,
Section 4.3 shows the implementations in HyRoDyn, and Section 4.4 and 4.5 are reserved
for case studies.
4.1 A Layer between Implicit and Explicit Form

The hybrid version can be thought of as having a layer between the numerical implicit
and analytical explicit constraints. The layer between our constraints is responsible for
extracting the explicit constraints from the implicit constraints numerically. Figure 4.1
gives the overview of the approach.
Calculate loop closure

Calculate loop closure Calculate acceleration
constraints on position
constraints on velocity level constraints
level numerically
level to nd matrix K numerically k
(q) = 0
Find numerically
using Recursive Newton Extract matrix G from K Extract vector g from k
-Raphson method
Calculate loop closure Calculate loop closure Calculate acceleration

constraints on position constraints on velocity level constraints
level analytically level analytically to nd analytically g
q= (y) matrix G
Figure 4.1: Extraction of explicit constraints from the implicit constraints
In Figure 4.1, the blue colored cells in the first row follow the implicit constraints.
Similarly, the orange colored cells in the last row follow the explicit constraints. A layer
has been introduced between them whose objective is to provide the explicit constraints
from implicit constraints.
4.2 Further Developments in User Interface

In terms of software architectures, HyRoDyn provides the interfaces through Unified Robot
Description Format(URDF) and YAML Ain’t Markup Language (YAML) based submech-
anism files. The user is expected to provide the inputs as these two files for modeling of
34
rigid body systems. URDF is an XML(extensible Markup Language) format that is used
in ROS and is widely used in the robotics community. However, URDF misses on a key
feature, i.e., it does not allow the definition of closed loops in the system. To overcome
this limitation, [14] provides the support through YAML file where the user can provide
the required information to formulate loop closures in explicit form.
The submechanism YAML file expects the user to input certain parameters for the
submechanism. These parameters include name, type, contextual name, the file path of
the URDF, active joint names, independent joint names, and spanning tree joint names.
The submechanism definitions should be coherent with the joint names in the URDF file.
A simple example of the submechanism file is shown below.
submechanisms :
− name : < SUBMECHANISM NAME >
type : < SUBMECHANISM NAME >
contextual name :
f i l e p a t h : < PATH TO SUBMECHANISM URDF >
j o i n t n a m e s : < ALL JOINTS IN THE URDF FILE >
j o i n t n a m e s a c t i v e : [ <J1 > ,.. , <JP> ]
j o i n t n a m e s i n d e p e n d e n t : [ <J1 > ,.. , <JM> ]
j o i n t n a m e s s p a n n i n g t r e e : [ <J1 > ,.. , <JN> ]
From the inputs above, type acts as a key value for mapping to the submechanism libraries
in the HyRoDyn software database. The type of submechanism should be chosen from
the database. Currently, the database supports RRPR, 2SPU+1U, 2SPRR+1U, 6UPS,
PARALLELOGRAMCHAIN, and TRANSMISSION libraries.
In a further development to the user interface, the submechanism YAML file is ex-
tended to provide support for the numerical approach. The definition of loop constraints
is required for the numerical approach. The key value that is used to switch between the
numerical and analytical approach is through the node name type in the submechanism
YAML file. The numerical approach can be used for a closed loop system by changing
the node name to type: ”NUMERICAL”. This directs the software to find the explicit
constraints numerically.
The definition of loop constraints in the YAML file includes various parameters which
include the name of the cut joint frame, predecessor body of the cut joint, successor body
of the cut joint, and constraints. For each loop constraint, the user can input the name,
constraint axis, or the direction in which the constraints need to be put. Note that the
constraint axis should be defined with respect to the local frame of the cut joint and should
be defined as a spatial vector. The developments in the submechanism YAML file can be
seen below.
35
submechanisms :
− name : < SUBMECHANISM NAME >
type : < SUBMECHANISM NAME / NUMERICAL >
contextual name :
f i l e p a t h : < PATH TO SUBMECHANISM URDF >
j o i n t n a m e s : < ALL JOINTS IN THE URDF FILE >
j o i n t n a m e s a c t i v e : [ <J1 > ,.. , <JP> ]
j o i n t n a m e s i n d e p e n d e n t : [ <J1 > ,.. , <JM> ]
j o i n t n a m e s s p a n n i n g t r e e : [ <J1 > ,.. , <JN> ]
loop constraints :
− c u t j o i n t : < CUT JOINT FRAME>
p r e d e c e s s o r b o d y : < PREDECESSOR BODY NAME >
s u c c e s s o r b o d y : < SUCCESSOR BODY NAME >
constraint axes :
− name : < CONSTRAINT NAME >
a x i s : [< 6D SPATIAL VECTOR >]
b a u m g a r t e s t a b i l i z a t i o n p a r a m e t e r : < VALUE >
This concludes the developments in the user interface in HyRoDyn software.
4.3 Implementation of Hybrid Version in HyRoDyn

The HyRoDyn software is implemented in C++ and uses the implementation of the multi-
body dynamics from Rigid Body Dynamics Library(RBDL) [8]. The software has Eigen3
and yaml-cpp as dependencies. Eigen3 is required for matrices and vector computations
and yaml-cpp is required for parsing the input submechanism YAML file. The software
consists of submechanism libraries for parallel mechanisms. Each library contains the sym-
bolic code for a particular submechanism type. An abstract class ExplicitLoopConstraintSet
is defined. Each submechanism library inherits from this abstract class. Similarly, a new
class NumericalLoopConstraints is defined that inherits from this abstract class.
As seen in Figure 4.2, the submechanism libraries 1RRPR, 2SPRR+1U, and Numer-
icalLoopConstraints is inheriting from the abstract class ExplicitLoopConstraintSet. Each
class has their own member variables and member functions. But, each submechanism
library overloads the member functions represented in red color in the image. The new
class NumericalLoopConstraints also overloads these member functions as they define the
explicit constraints for the parallel submechanism.
The constructor of the NumericalLoopConstraints class takes arguments that include

the file path of the URDF, various joint names, and loop constraint set. The loop con-
straint set is passed through a YAML parser from the submechanism YAML file. The
36
constructor is responsible to attach the loop constraints to the model. As discussed in the
previous chapter, loop constraints require parameters like body id’s, cut joint frames, con-
straint axis, and relative transformations. Although, most of the information is provided
in the submechanism YAML file, relative transformations between cut joint frame and
predecessor body, and relative transformation between cut joint frame and successor body
are required to be computed in the constructor of the class. This feature is user-friendly
as the transformation matrices can be cumbersome to write. The relative transformations
can be found as
0
T p(k) p(k) T k = 0 T k (4.1)
p(k)
T k = (0 T −1 0
p(k) ) T k (4.2)
0
T s(k) s(k) T k = 0 T k (4.3)
s(k)
T k = (0 T −1 0
s(k) ) T k (4.4)
where p(k) T k ∈ SE(3) is the relative transformation between the predecessor body and
the cut joint frame. Similarly, s(k) T k ∈ SE(3) is the relative transformation between the
successor body and cut joint frame.
When the user specifies cut joint frame, predecessor body, and successor body, the
transformations 0 T p(k) , 0 T s(k) , and 0 T k are calculated. Here, k is represents the cut joint
frame, p(k) denotes the predecessor body, and s(k) denotes the successor body. For a de-
fined model, it is fairly easy to compute the transformation from base to any frame using
RBDL.
The constructor is also responsible for calculating the selection matrices Qi and Qd .
Qi is the independent joints selection matrix of size m × n, and Qd is the dependent joint
selection matrix of size (n − m) × n.
The overloaded functions calc loopclosure Jacobian() uses the Algorithm 1 to compute
explicit constraint jacobian matrix and calc loopclosure g() uses the Algorithm 2 to com-
pute explicit bias acceleration vector.
37
Class Name oopConstraintSet
+ active_dof: unsigned int

Member + independent_dof: unsigned int
+ spanningtree_dof: unsigned int
Variables + selection_matrix: MatrixXd
+ EnalyticalLoopConstraintSet()
+ calc_loopclosure_function(y: VectorXd): VectorXd
+ calc_loopclosure_Jacobian(y: VectorXd): MatrixXd
+ calc_loopclosure_Jacobiand(y: VectorXd, yd: VectorXd): MatrixXd
Member + calc_loopclosure_g(y: VectorXd, yd: VectorXd): VectorXd
+ get_active_dof(): unsigned int
Functions + get_independent_dof(): unsigned int
+ get_spanningtree_dof(): unsigned int
+ get_selection_matrix(): MatrixXd
1RRPR 2SPRR+1U NumericalLoopConstraints

+ m : Model
+ cs : ConstraintSet
- l1: double - s1, s2: Vector3d + K, independent_joints_selection_matrix,
- l2: double - f1, f2: Vector3d dependent_joints_selection_matrix : MatrixNd
- d: double - r: double + Q, QDot, QInit, weights : VectorNd
+ G, Gi, Gd, Ki, Kd : MatrixXd
- n1, n2: Vector3d
+ 1RRPR( le_path: string) + g, gi, gd : VectorXd
+ calc_loopclosure_function + 2SPRR+1U(le_path: string) + NumericalLoopConstraints(le_path: string,
(y: VectorXd): VectorXd jointnames_spanningtree: vector<string>,
+ calc_loopclosure_Jacobian
+ calc_loopclosure_function
jointnames_independent: vector<string>,
(y: VectorXd): MatrixXd (y: VectorXd): VectorXd jointnames_active: vector<string>,
+ calc_loopclosure_Jacobiand + calc_loopclosure_Jacobian loop_contraints_set: vector<LoopConstraints>)
+ calc_loopclosure_function(y: VectorXd): VectorXd
(y: VectorXd, yd: VectorXd) (y: VectorXd): MatrixXd + calc_loopclosure_Jacobian(y: VectorXd): MatrixXd
+ calc_loopclosure_g(y: VectorXd, + calc_loopclosure_Jacobiand + calc_loopclosure_g(y: VectorXd, yd: VectorXd): VectorXd
yd: VectorXd): VectorXd (y: VectorXd, yd: VectorXd) + calc_G_from_K(K: MatrixNd) : MatrixXd
+ calc_k(m: Model, cs: ConstriantSet, Q: VectorNd,
+ calc_loopclosure_g(y: VectorXd, QDot: VectorNd) : VectorXd
yd: VectorXd): VectorXd + calc_g_from_k(K: MatrixNd, k: VectorXd): VectorXd
+ calc_selection_matrices( jointnames_spanningtree:
vector<string>, jointnames_independent:
vector<string>)
+ calc_permutationmatrix(jointnames_spanningtree:
vector<string>, jointnames_active:
vector<string>)
Figure 4.2: NumericalLoopConstraints class in HyRoDyn, adapted from [13]
38
4.4 Case Study: Multi Loop Mechanism
The multi loop mechanism considered in this case study is taken from the humanoid robot
RH5v2 at DFKI GmbH.
End-Effector Link
End-Effector Joints
Base Joints
Base Link
(a) CAD prototype of Torso mech-

anism (Credits: Heiner Peters, (b) Simple representation of the
DFKI GmbH) parallel mechanism
Figure 4.3: 2SPU+1U mechanism [13]
The 2SPU+1U mechanism in Figure 4.3 consists of a spherical joint, prismatic joint,
and an universal joint in both the loops between base and end-effector. The base and
end-effector are connected by a universal joint. The independent joints are represented
in green color, active joints are represented in red color, and remaining passive joints are
represented in yellow color in the image.
A topological graph is constructed for the above mechanism by considering the spherical
joint as the cut joint.
BodyRoot
BodyBL1 BodyBR1
3 6
1 BodyPitch
BodyBL2 4 7 BodyBR2
BodyActL 5 BodyRoll 8 BodyActR

2
Figure 4.4: Topological graph of 2SPU+1U mechanism
In Figure 4.4, red lines indicate the active joints, and green lines indicate the inde-
pendent joints. The cut joint, the spherical joint, in this case, is represented by a dotted
line. The loop constraints shall be defined in the submechanism YAML file. The cut joint
39
frames are BodyActL and BodyActR. The predecessor and successor body for the cut joint
BodyActL are BodyActL Link and BodyRoll Link respectively. Similarly, on the right side
of the topological graph, the predecessor and successor body for the cut joint BodyActR
are BodyActR Link and BodyRoll Link respectively. As the cut joint is a spherical joint,
the position constraints exist in x, y, and z directions for both loops on the left and right
sides of the topological graph.
The YAML file for this submechanism can be seen in Figure 4.5 below.
BodyRoot
BodyBL1 BodyBR1
3 6
1 BodyPitch
BodyBL2 4 7 BodyBR2
BodyActL 5 BodyRoll 8 BodyActR

2
Figure 4.5: Topological graph and submechanism definition of 2SPU+1U mechanism
Note that in Figure 4.5, node name type is changed to ”NUMERICAL”.

The constructor of NumericalLoopConstraints class computes the relative transformations
between the bodies and cut joint frames. The torso submechanism consist of n = 8
spanning-tree joints, m = 2 independent joints, and n − m = 6 dependent joints. The
selection matrices are computed with this information.
Independent joint selection matrix, Qi (2 × 8) is calculated as

1 0 0 0 0 0 0 0
Qi = (4.5)
0 1 0 0 0 0 0 0
40
Dependent joint selection matrix, Qd (6 × 8) is calculates as
 
0 0 1 0 0 0 0 0
0 0 0 1 0 0 0 0
 
0 0 0 0 1 0 0 0
Qd =  0 0 0
 (4.6)
 0 0 1 0 0

0 0 0 0 0 0 1 0
0 0 0 0 0 0 0 1
4.5 Case Study: Reduced Version of the Upper Body of

Humanoid RH5v2
A case study of reduced version of the RH5v2 robot is considered. The reduced version of
the robot is shown in Figure 4.6.
(a) Front view of the reduced version (b) Back view of the reduced version
Figure 4.6: Reduced version of RH5v2
The whole system consists of 8 submechanisms which are:

1. Torso Body: It is a parallel kinematic chain of the type 2SPU+1U. It is a multi-
loop mechanism. Here, the independent or input joint is the universal joint and the
actuated joint is left and right prismatic actuator. This can be seen previously in
Figure 4.4. Section 4.4 already provides the detailed information of the mechanism.
2. Body Yaw: It is a serial chain with single revolute joint.
3. Left shoulder: It is a serial chain with 3 revolute joints.
4. Left elbow: It is a parallel kinematic chain of the type RRPR or lambda mechanism.
The independent joint is the revolute joint denoted in green and the actuated joint
41
is the prismatic joint denoted in red. This can be visualized in Figure 4.7. The cut
joint considered here is revolute joint, defining the cut joint frame as ALElbowAct.
The position constraints exist in y and z direction after referring to the URDF file.
The predecessor and successor body are ALElbow Link and ALElbowAct Link re-
spectively.
ALElbowB11
2
ALElbow
ALElbowAct
3 1
Figure 4.7: Topological graph of RRPR mechanism
5. Left wrist roll: It is a serial chain with single revolute joint.
6. Right shoulder: It is a serial chain with 3 revolute joints.
7. Right elbow: It is also a parallel kinematic chain of the type RRPR or lambda
mechanism as described before in Figure 4.7
8. Right wrist roll: It is a serial chain with single revolute joint.
Figure 4.8 gives the details about the topological graph of the whole system and the
respective definitions of the submechanisms in YAML file.
42
BodyRoot
BodyBL1 BodyBR1
BodyPitch
BodyBL2 BodyBR2
BodyActL BodyActR
BodyRoll
BodyYaw
Connector_Body/Torso
ALBase ARBase
ALShoulder1 ARShoulder1
ALElbowB11
ARElbowB11
ALElbowAct ALElbow ARElbow ARElbowAct
ALWristRoll ARWristRoll
Figure 4.8: Spanning tree with submechanism definitions
43
Chapter 5
Constraint Embedding for Forward

Dynamics
This chapter is dedicated to the forward dynamics algorithm for closed loop systems. The
forward dynamics is the computation of accelerations of the multi-body system given the
actuated forces. The forward dynamics problem for tree type systems can be solved in
mainly two ways [6],
1. Formulate equations of motion for the rigid body system and solve for acceleration.
2. Propagate through each body such that the accelerations are computed for one joint
at a time.
In the first approach, the n×n joint space inertia matrix is calculated for the whole system,
where n is the number of spanning-tree joints in the topological graph. The whole set of
equations of motion are solved for accelerations as a set of n linear equations by inversion
of joint-space mass inertia matrix. These algorithms generally have the complexity as
O(n3 ). On the other hand, the second method propagates through each body and computes
the accelerations considering joint constraints. These algorithms have the complexity of
O(n). The Featherstone algorithm, also called as Articulated Body algorithm is a recursive
forward dynamics algorithm for tree type systems that implements second method and was
introduced in [7]. The articulated body algorithm (ABA) is efficient when the rigid body
system has a large number of bodies. For smaller rigid body systems, the first approach
can match or even outperform the ABA algorithm. When there are closed loops in the
system, if the first approach is preferred for larger systems, the forward dynamics can
become inefficient due to inversion of large mass inertia matrix.
However, for closed loop systems, [10] presents recursive algorithms for forward dynamics.
The approach is called constraint embedding. The aim of this chapter is to adapt the
constraint embedding approach in the ABA algorithm from [6]. The modified ABA does
not have O(n) complexity. The computational complexity depends on the loop closures in
44
the system. The chapter is divided into sections where Section 5.1 shows the computations
of articulated body inertia required for ABA algorithm, Section 5.2 presents the ABA
algorithm from [6], Section 5.3 gives the idea of embedding loop constraints from [10],
Section 5.4 shows the modifications in ABA algorithm to handle loop constraints. Last
Section 5.5 presents the example.
5.1 Articulated Body Inertia

Articulated body inertia is a major component required in the ABA algorithm. In [6],
articulated body inertia is defined as the perceived inertia by a body in the rigid body
system. Consider a rigid body B in space with applied force f and resulting acceleration
a. The equation of motion for the body is:
f = Ia + p (5.1)
where I and p are the rigid body inertia and bias force respectively. When another body
B 0 is attached to the body B through a joint, the body is perceived to have articulated
body inertia. The articulated-body equation of motion can be written as:
f = I A a + pA (5.2)
where I A and pA are the articulated-body inertia and bias force of body B when B 0 is
attached through a connecting joint respectively. B is called the handle and the system
containing both the bodies is the articulated body.
The basic properties of the articulated body are summarized below.
1. The kinematic tree is a floating kinematic tree, i.e. articulated body has no connection
to the fixed base.
2. Every articulated body has exactly one handle.
3. Every handle has full six degrees of freedom.
4. Every assembly operation consists of connecting one handle to another via a single
joint.
Calculation of Articulated Body Inertia

The calculation of articulated body inertia is taken from [6]. Consider two articulated
bodies B1 and B2 in Figure 5.1. The applied forces acting on bodies B1 and B2 are f 1
and f 2 respectively. The resulting accelerations are denoted by a1 and a2 . The equations
of motion are given by
f 1 = IA A
1 a1 + p1 (5.3)
f 2 = IA A
2 a2 + p2 (5.4)
45
where I A A
1 and I 2 are the articulated-body inertia of bodies B1 and B2 respectively. The
bias forces are given by pA A
1 and p2 for bodies B1 and B2 respectively.
X2
B2 B1 fJ
B1
f2 a2 f a
f1 a1
Figure 5.1: Construction of new articulated body, adapted from [6]
When the joint screw coordinate vector X (in body-fixed representation) connects the
bodies, a new unknown force, f J arises, transmitting the force from one body to another.
The equations describing the relationship between the forces is given by
f1 = f − fJ and f 2 = f J (5.5)
The unknown force imposes constraints on the system,
a2 − a1 = X q̈ + c and τ = X T f J (5.6)
where c = Ẋ q̇.
Substituting the values,
τ = XT f 2
= X T (I A A
2 a2 + p2 )
= X T (I A A
2 (a1 + X q̈ + c) + p2 )
So,
−1
q̈ = (X T I A T A A
2 X) (τ − S (I 2 (a1 + c) + p2 )) (5.7)
Solving for q̈, the equation of motion relating f and a1 ,
f = f1 + f2
= IA A A A
1 a1 + I 2 a2 + p1 + p2
= IA A A
1 a1 + I 2 (a1 + X q̈ + c) + p1 + p2
−1
= IA T A T A A A A
1 a1 + I 2 (a1 + c + X(X I 2 X) (τ − S (I 2 (a1 + c) + p2 ))) + p1 + p2
Comparing it with equation f = I A a1 + pA ,

−1 T A
IA = IA A A T A
1 + I 2 − I 2 X(X I 2 X) X I 2
46
and
−1
pA = pA A A A T A T A A
1 + p2 + I 2 c + I 2 X(X I 2 X) (τ − S (I 2 c + p2 )) (5.8)
where I A and pA are the computed articulated body inertia and spatial bias force when
B2 is attached to the handle B1 via a connecting joint.
5.2 Articulated Body Algorithm

The ABA algorithm calculates forward dynamics by computing the articulated body inertia
in the previous section. The algorithm makes three passes over the tree-type system. The
passes are as follows
1. The first pass goes from base to tip of the kinematic tree and is responsible for
calculating the velocity and bias terms of each node in the kinematic tree.
2. The second pass runs from tip to base and calculates the articulated body inertia
and bias forces.
3. The third pass goes from base to tip and computes the accelerations.
The algorithm is described in Algorithm 3 [6].
5.3 Introduction to Constraint Embedding

This section presents the adaptation of the ABA algorithm to deal with loop closures or
parallel kinematic chain [10]. Consider a simple kinematic tree single closed loop in Figure
5.2. The cut joint is represented with a dashed line in the figure.
Figure 5.2: Kinematic tree with single loop
47
Algorithm 3 Articulated body algorithm
1: v 0 = 0
2: for i = 1 to NB do
3: [AdJ , X i , v J , cJ ] = jcalc(jtype(i), q i , q̇ i )
4: AdTλ(i),i = AdJ AdT (i)
5: if λ(i) 6= 0 then
6: AdT0,i = AdT0,λ(i) AdTλ(i),i
7: end if
8: v i = AdTλ(i),i v λ(i) + v J
9: ci = cJ + v i × v J
10: IAi = Ii
T x
11: pAi = adv i I i v i − AdTi,0 f i
12: end for
13: for i = NB to 1 do
14: U i = IA i Xi
15: D i = X Ti U i
16: ui = τ i − X Ti pi
17: if λ(i) 6= 0 then
−1 T
18: Ia = IA i − U i Di U i
a −1
19: pa = pA i + I ci + U i D i u i
T
20: A A
I λ(i) = I λ(i) + AdTλ(i),i I a AdTλ(i),i
T
21: pA A
λ(i) = pλ(i) + AdTλ(i),i p
a
22: end if
23: end for
24: a0 = −ag
25: for i = 1 to NB do
26: a0 = AdTλ(i),i aλ(i) + ci
27: q̈ i = D −1 T 0
i (ui − U i a )
28: 0
ai = a + X i q̈ i
29: end for
48
The spanning tree can be modified by considering the loop closure as a node. This
converts the spanning tree into a new spanning tree with the submechanism captured in a
new node. The approach removes the bodies involved in the parallel kinematic chain and
introduces a new submechanism node in the graph, G. The explicit loop closure constraints
should be captured in this submechanism node.
c c
p p
Figure 5.3: Introduction of a submechanism node in the tree, adapted from [10]
The modified spanning tree has the submechanism node G with its parent p and child
c. This is depicted in Figure 5.3. The constraints should be embedded in the spanning tree
when going from p → G → c. The next section shows how the constraints can be defined.
5.4 Constraint Embedding in Articulated Body Algorithm

As discussed in previous chapters, explicit Jacobian matrix GG and explicit bias accelera-
tion vector g G can be computed for a parallel submechanism in the tree. The new graph
introduces connector blocks for computing articulated body inertia and bias force for the
submechanism node G. These are defined as
1. Connecting matrix from p → G called as A(p, G)
2. Connecting matrix from G → c called as A(G, c)
3. Explicit Jacobian matrix in reduced coordinates J y . The new submechanism node G

represents the loop closure and contains nodes from the kinematic tree. The reduced
jacobian matrix is matrix of the tree structure in node G constrained with the explicit
jacobian matrix GG .
J y = AG X G G G (5.9)
where AG and X G are defined in Equation 2.20.
49
The first pass in the algorithm remains the same and computes velocities and bias terms.
The second loop is modified when moving from tip to base in the kinematic tree. The
modifications are listed below.
From child node c to submechanism node G (c → G)
X
IA
G = IG + AT (G, c)I ac A(G, c)
∀c∈µ(G)
X
pA
G = pG + AT (G, c)pac
∀c∈µ(G)
UG = IA
GJ y
DG = J Ty U G
uG = τ Gy − J Ty pA
G
I aG = I G − U G D −1 T
G UG
c0G = AG cG + AG X G g G
a 0 −1
paG = pA
G + I G cG + U G D G uG
From submechanism node G to parent node p (G → p)

IA T a
p = A (p, G)I G A(p, G) + I p
pA T a
p = A (p, G)pG + pp
Considering n bodies and m independent coordinates in submechanism node G,

• I G is a 6n × 6n matrix of the submechanism node with diagonal terms as spatial
mass-inertia matrix of each body in the node G.
• IA
G is the 6n × 6n articulated body inertia of node G. Connecting matrix A(G, c)
maps child to the connecting body in node G.
• pG is a 6n vector of articulated bias forces of the submechanism node.
• J y is the reduced jacobian matrix in explicit form of size 6n × m.
• τ Gy is a m vector of generalized forces of the node in independent coordinates.
The next modifications in third loop are given below when moving from base to tip. The
modifications are summarised below.
From parent node p to submechanism node G (p → G)
a0G = A(p, G)ap + c0G
ÿ = D −1 T 0
G (uG − U G aG )
q̈ G = GG ÿ + g G
aG = a0G + J y ÿ
50
From submechanism node G to child node c (G → c)
a0c = A(G, c)aG + cc

q̈ c = D −1 T 0
c (uc − U c ac )
This concludes the constraint embedding approach in articulated body algorithm.
5.5 Example
The demonstration can be shown with a simple example of a series-parallel hybrid chain
with three submechanisms. The first submechanism is a 3R serial chain with three revolute
joints, the second submechanism is a lambda mechanism or RRPR mechanism (parallel
kinematic chain with a single loop), and the third submechanism is a serial chain with a
single revolute joint. the topological graph of the series-parallel hybrid example is shown
in Figure 5.4.
Root
ALShoulder1
1
ALShoulder2
2
ALShoulder3
ALElbowB11 3
5
G ALElbowAct ALElbow
6
4
ALWristRoll
Figure 5.4: Topological graph of 3R-Lambda-R chain
From Figure 5.4, the submechanism node can be created for the parallel kinematic chain
of lambda mechanism. The submechanism node consists of three bodies as G = {4, 5, 6}.
For this submechanism node G, parent node is p = 3, and child node is c = 7.
51
The components can be computed for node G as,
 
I4 0 0
IG =  0 I5 0  (5.10)
0 0 I6
where I i is the spatial inertia matrix of body i.

 
p4
pG = p5 
 (5.11)
p6
where pi is the spatial bias force of body i.

 
I 0 0
AG = 0 I 0 (5.12)
0 AdT5,6 I
where I is the Identity matrix.

 
X4 0 0
XG =  0 X5 0  (5.13)
0 0 X6
where X i is the screw coordinate vector of joint i in body-fixed representation.

Connecting matrix are very important for mapping of components from submechanism
node to either parent node or child node. For this example,
 
I 0 0
A(p, G) = AdT3,4 AdT3,5 0 0 I 0 (5.14)
0 AdT5,6 I
 
AdT4,7
A(G, c) =  0  (5.15)
0
Substituting these matrices, the constraint embedding can be performed for the closed loop
system in Figure 5.4.
52
Chapter 6
Validation and Results
This chapter validates the hybrid numerical and analytical approach for resolving loop clo-
sure equations in series-parallel hybrid robots. The extraction of explicit constraints from
the implicit constraints is tested on a series-parallel hybrid RH5v2 robot at DFKI Robotics
Innovation Center, Germany. The best way to check the numerical implementation of ex-
plicit constraints is to compare with an already available analytical approach in HyRoDyn
software. The explicit constraints in the analytical form are available for some parallel
submechanisms in the RH5v2 robot in the HyRoDyn software. If the results from numer-
ical and analytical approaches matches, it can be maintained that the numerical approach
is implemented correctly. This provides the confidence to use a numerical approach for
some parallel mechanisms whose symbolic expressions are not available as a submechanism
library in HyRoDyn software. The hybrid numerical and analytical approach is tested in
both simulation and real robot for defined trajectories.
After successful validation, the computational efficiency can be measured for a purely nu-
merical approach and purely analytical approach. The numerical approach is expected to
be slower than the analytical approach. However, the numerical approach should not be
very slow in terms of computation. A scale of 50 ∼ 100 times the analytical approach is
considered acceptable for the numerical approach. As the numerical approach is expected
to be used only if the symbolic expressions are not available for a parallel submechanism,
the hybrid version is expected to have higher computational efficiency than the purely
numerical approach. The computation timings are presented as a result of the hybrid nu-
merical and analytical approach.
The validation of constraint embedding in articulated body algorithm is also performed
and the accelerations are computed for provided actuated torques. The validation is per-
formed with the direct approach of inverting the joint-space mass inertia matrix. The
forward dynamics algorithm using the constraint embedding approach is considered valid
if the accelerations computation matches with the direct method.
This chapter is divided into several sections where Section 6.1 discusses the simulation re-
53
sults, Section 6.2 presents experimental results, Section 6.3 presents computation timings,
and Section 6.4 shows the validation of constraint embedding method.
6.1 Simulation Results

For comparing the results in the simulation, the reduced version of the upper body is con-
sidered. A simple walk-through is presented in Section 4.5. The topological graph can be
seen in Figure 4.8. The topological graph is shown again for easier access in Figure 6.1
below.
BodyRoot
BodyBL1 BodyBR1
BodyPitch
BodyBL2 BodyBR2
BodyActL BodyActR
BodyRoll
BodyYaw
Connector_Body/Torso
ALBase ARBase
ALElbowB11
ARElbowB11
ALElbowAct ALElbow ARElbow ARElbowAct
ALWristRoll ARWristRoll
Figure 6.1: Spanning tree of the reduced version of RH5v2 upper body
In the topological graph in Figure 6.1, the left arm in the body consists of five inde-
pendent joints (3 for shoulder, 1 for elbow, 1 for wrist roll). The right arm also consists of
five independent joints. The torso consists of 3 independent joints. Therefore, the reduced
version of the RH5v2 upper body has a total of y = 13 independent joints. The whole
spanning tree consists of a total of 23 spanning tree joints, and 13 actuated joints. A total
of nc = 10 constraints need to be defined for solving loop closures in the system.
54
The input trajectory is defined for the 13 independent joints in the spanning tree. The cy-
cloidal trajectories [15] are defined for these joints. The generalized positions y, generalized
velocities ẏ, and generalized accelerations ÿ are defined for a single time step as
ymax − ymin Tf 2π
y = ymin + ( )(t − sin t) (6.1)
Tf 2π Tf
ymax − ymin 2π
ẏ = ( )(1 − cos t) (6.2)
Tf Tf
ymax − ymin 2π 2π
ÿ = ( ) sin t (6.3)
Tf Tf Tf
where
• ymin is the minimum joint limit of the joint
• ymax is the maximum joint limit of the joint
• Tf is the final simulation time.

From the whole spanning tree, the actuated joints of the parallel kinematic chains are
chosen for the comparison between purely numerical and purely analytical approach for
resolving loop closure equations. These joints include q5 and q8 from the torso mechanism,
q3 from the left and right lambda mechanism.
Position Plots Comparison

The position for the parallel mechanism actuated joints are plotted as in Figure 6.2 below.
Position anal ses

Q5_anal tical
Q5_numerical
0.06 Q8_anal tical
Q8_numerical
Q3Lrrpr_anal tical
0.04 Q3_Lrrpr_numerical
Q3Rrrpr_anal tical
Q3_Rrrpr_numerical
Position (m or rad)
0.02
0.00
−0.02
−0.04
−0.06
0 20 40 60 80 100
Time(s)
Figure 6.2: Position plots
55
Velocity Plots Comparison
The velocity plots are provided for the same joints as in position plots in Figure 6.3.
Velocity analyses
Q5d_analytical
Q5d_numerical
0.0025 Q8d_analytical
Q8d_numerical
Q3dLrrpr_analytical
0.0020 Q3d_Lrrpr_numerical
Q3dRrrpr_analytical
Velocity (m/s or rad/s)
Q3d_Rrrpr_numerical
0.0015
0.0010
0.0005
0.0000
0 20 40 60 80 100
Time(s)
Figure 6.3: Velocity plots
Acceleration Plot Comparison

The acceleration plots can be seen below in Figure 6.4 for the same actuated joints.
Acceleration analyses
0.000100
Q5dd_analytical
Q5dd_numerical
0.000075 Q8dd_analytical
Q8dd_numerical
Q3ddLrrpr_analytical
0.000050 Q3dd_Lrrpr_numerical
Acceleration (m/s^2 or rad/s^2)
Q3ddRrrpr_analytical
Q3dd_Rrrpr_numerical
0.000025
0.000000
−0.000025
−0.000050
−0.000075
0 20 40 60 80 100
Time(s)
Figure 6.4: Acceleration plots
56
Actuated Torque Plot Comparison
The actuated torques for the parallel submechanism can be seen in Figure 6.5 below.
Torque analyses
150
100
50
Torque (Nm) or Force (N)
−50
tau_torso_L_analytical
tau_torso_L_numerical
tau_torso_R_analytical
−100
tau_torso_R_numerical
tau_Lrrpr_analytical
tau_Lrrpr_numerical
−150 tau_Rrrpr_analytical
tau_Rrrpr_numerical
0 10 20 30 40 50 60
Time(s)
Figure 6.5: Torque plots
As the plots in Figures 6.2, 6.3, 6.4, and 6.5 matches correctly, it provides a successful
validation for the numerical implementation in HyRoDyn software.
6.2 Experimental Results

For the experimental results, Rh5v2 robot is shown in Figure 6.6.
Figure 6.6: Rh5v2 Humanoid robot
57
The input trajectories on the real system are different from the cycloidal trajectories.
For experimental verification, the trajectories are provided for boxing motion. Some of the
configurations in the trajectories are shown in Figure 6.7. Independent joints are provided
with the trajectories and the data is collected for both approaches, i.e. purely analytical
and purely numerical.
(a) Initial configuration of the (b) Intermediate configuration in

robot the motion
(c) Another configuration in the

motion
Figure 6.7: Various configurations of RH5v2
58
Position Plots Comparison
The position for the parallel mechanism actuated joints are plotted as in Figure 6.8 below.
Position analyses
0.02
0.00
Q5_analytical
−0.02
Position (m or rad)
Q5_numerical
Q8_analytical
Q8_numerical
−0.04
Q3_Lrrpr_analytical
Q3_Lrrpr_numerical
Q3_Rrrpr_analytical
−0.06
Q3_Rrrpr_numerical
−0.08
−0.10
0 1 2 3 4 5
time(s)
Figure 6.8: Position plots using experimental data
Velocity Plots Comparison

The velocity plots are provided for the same joints as in position plots in Figure 6.9.
Velocity analyses
0.10
0.05
Velocity (m/s or rad/s)
0.00
−0.05 Qd5_analytical
Qd5_numerical
Qd8_analytical
Qd8_numerical
−0.10 Qd3_Lrrpr_analytical
Qd3_Lrrpr_numerical
Qd3_Rrrpr_analytical
−0.15 Qd3_Rrrpr_numerical
0 1 2 3 4 5
time(s)
Figure 6.9: Velocity plots using experimental data
59
Actuated Torque Plot Comparison
The actuated torques for the parallel submechanism can be seen in Figure 6.10 below.
Torque analyses
−200
Torque (Nm or N)
−400
tau5_analytical
tau5_numerical
tau8_analytical
tau8_numerical
−600
tau3_Lrrpr_analytical
tau3_Lrrpr_numerical
tau3_Rrrpr_analytical
tau3_Rrrpr_numerical
0 1 2 3 4 5
time(s)
Figure 6.10: Torque plots using experimental data
From Figures 6.8, 6.9, and 6.10, it can be seen that there exists small error in the
motion on the real system. These errors arise due to bad tracking of the trajectories on
the real system.
6.3 Computational Performance

The computational performance is measured in CPU time for the reduced version of the
RH5v2 humanoid robot in Figure 6.1. For this robot with 23 spanning tree joints, time is
computed for position, velocity, acceleration, and torque analysis for randomly generated
trajectories. A total number of 10000 calls were made to solve the system state (kinemat-
ics), inverse dynamics, and forward dynamics. The program is run on a standard laptop
with an Intel Core i7 CPU @ 2.8 GHz. For computational performance, three cases are
studied where
1. The full system is solved through numerical approach
2. The full system is solved through analytical approach
3. The full system is solved through hybrid approach where torso mechanism (Figure
4.4) is solved numerically and other parallel mechanisms are solved analytically.
60
Table 6.1: Computation time for analysis
Analysis Numerical time(s) Analytical time(s) Hybrid time(s)

System state 7.4351e-05 4.72069e-06 5.41666e-05
Inverse Dynamics 8.4802e-05 1.59151e-05 6.52376e-05
Forward Dynamics 9.83404e-05 2.28143e-05 7.68844e-05
As seen in Table 6.1, the computational performance for the numerical approach is ∼ 6
times slower than the analytical approach. Also, it can be noted that hybrid version per-
forms better than the purely numerical approach. This is well within the defined objective
of 50 ∼ 100, and hence the numerical and hybrid approaches for resolving loop closures is
efficient. The computational timings for purely analytical, purely numerical, and hybrid
approach from Table 6.1 are plotted in Figure 6.11.
Computational performance comparison

Numerical approach
Forward dynamics
Hybrid approach
Analytical approach
Inverse dynamics
Type of Analysis
System State
0.00000 0.00002 0.00004 0.00006 0.00008 0.00010

Average computaion time (s)
Figure 6.11: Comparison of different approaches for resolving loop closure constraints
In Figure 6.11, it is quite evident that the hybrid approach takes lesser time when
compared to purely numerical approach. It is expected to be slower than purely analytical
approach.
Therefore, the hybrid numerical and analytical approach is the best for resolving loop
closure equations in HyRoDyn software when the symbolic codes are not available for a
submechanism.
Computational Performance for New Parallel Mechanism

The computational performance is studied for a rigid body system (61 spanning tree joints,
20 independent joints, and 20 actuated joints) with a parallel mechanism whose symbolic
61
expressions are not available yet in the database of HyRoDyn software. The mechanism is
shown in Figure 6.12.
Figure 6.12: Wrist mechanism in RH5v2 humanoid
The above shown wrist mechanism has 16 spanning tree joints, 2 independent joints,
2 active joints, and 4 loop closures. The mechanism is complex in terms of loop closures
where multiple cut joints define the constraints. The multiple cut joints in the mechanism
are two spherical cut joints and two planar revolute cut joints. The total number of nc = 10
constraints are required to solve the loop closures in the mechanism. The whole complex
system now require 30 constraints to be defined.
As it is quite difficult to generated symbolic codes for this parallel mechanism, numerical
approach is used to resolve the loop closures in this submechanism. The cases studied for
this model are
1. The full system is solved through numerical approach, including the wrist.
2. The full system is solved through hybrid approach where the numerical approach is
used only for the wrist mechanism. For other parallel submechanisms in the system,
analytical formulations are used.
The computational timings are captured in Table 6.2.
62
Table 6.2: Computation time where wrist is solved numerically
Analysis Numerical time(s) Hybrid time(s)

System state 0.000348103 0.000267049
Inverse Dynamics 0.000380755 0.000300842
Forward Dynamics 0.000459714 0.00037276
The computational timings are plotted in Figure 6.13.
Computational performance comparison

Numerical approach
Forward dynamics
Hybrid approach
Inverse dynamics
Type of Analysis
System State
0.0000 0.0001 0.0002 0.0003 0.0004

Average computaion time (s)
Figure 6.13: Comparison for wrist mechanism
As seen in Figure 6.13, the computational performance for the hybrid numerical and
analytical approach is better when compared to the purely numerical approach, thus val-
idating the main objective of the thesis. When the analytical formulations are generated,
the analytical solutions should be used due to their better computational efficiency.
6.4 Validation of Constraint Embedding Approach

The validation of constraint embedding approach is validated using the direct method of
inversion of joint-space mass inertia matrix of the whole system. The same example is
considered and the input trajectories for positions and velocities are given by cycloidal
trajectories. Also, random actuated torques are provided to the system. The accelerations
are computed through both the approaches and the results are compared.
63
Forward dynamic analyses
0
Accelerations (m/s^2 or rad/s^2)
−1
−2
Qdd5_direct
Qdd5_ABACE
−3
Qdd8_direct
Qdd8_ABACE
Qdd3_Lrr r_direct
−4 Qdd3_Lrr r_ABACE
Qdd3_Rrr r_direct
Qdd3_Rrr r_ABACE
0 20 40 60 80 100
Time(s)
Figure 6.14: Acceleration plots for constraint embedding validation
In Figure 6.14, the accelerations plots are shown for joints in the actuation space for
parallel submechanisms torso, left and right lambda mechanism. From the figure, it can
be confirmed that the constraint embedding formulations are implemented correctly.
This concludes the validation and results for the methods described in the thesis.
64
Chapter 7
Conclusions and Future Work
The objective of the master thesis was to explore a hybrid numerical and analytical ap-
proach for resolving loop closure equations in multi-body systems.
To achieve the objective, Chapter 2 introduces to modeling of rigid body systems. The
chapter briefly discusses the graph based topology for rigid body systems, gives the basic
background on screw theory and Lie group concepts, and gives the mathematical formula-
tions of equations of motion in the form of implicit and explicit constraints. The key rela-
tionship property between the implicit and explicit constraints is given and later exploited
in the following chapters. Chapter 3 gives the numerical approach for explicit constraints.
The chapter presents the extraction of explicit constraints from implicit constraints. After
forming the basis, Chapter 4 discusses the implementation of the numerical approach in
the HyRoDyn software at DFKI. The chapter shows the modifications in the user interface
and software architecture for handling both numerical and analytical approaches.
Next, forward dynamics algorithms to compute accelerations are shown for closed loop sys-
tems. Chapter 5 presents the modified recursive articulated body algorithm to handle loop
constraints in the rigid body system. The approach also uses explicit constraints to embed
constraints in the spanning tree. Finally, Chapter 6 asserts the validation and results of
the approach used in the thesis.
The final result of the thesis is an efficient hybrid approach to resolve loop closures in
the multi-body system. This approach significantly increases the generality of HyRoDyn
software which was able to handle submechanism known to its database. The work done
in the thesis can be successfully implemented for a new parallel mechanism in the closed
loop system.
Future Work
With the successful implementation of the hybrid numerical and analytical approach, the
future work is to have more general HyRoDyn software. The future work include
• The constraint embedding approach is mentioned in the thesis but its implementation
65
in the software needs improvement. There are some cases in the robot where the
current implementation can fail.
• The software must be able to handle contact constraints in the system. This will
further improve the generality where interaction with the environment is required.
• There is no method to check collisions in the system. Therefore, collision checks can
be added.
• The future work is also aimed to have wheeled robots integrated into the software.
This concludes the conclusion and future work aimed at after the thesis.
66
Appendix A
LUA File Extension for Defining

the Model
The RRPR model can be implemented using a Lua model file. In this file, different aspects
of the model like bodies, joints, meshes, and model are defined.
−− Parameters
−− Hip2 p a r a m e t e r s
l1 = 114.02/1000
l2 = 364.97/1000
J3 = 2 9 7 . 3 9 / 1 0 0 0
−− b a s e
m base = 1 . 0
r base = 0.02
I x x b a s e = m base ∗ l 1 ∗ l 1 / 3
Izz base = 0.0
−− B1
m B1 = 1.0
r B1 = 0.02
Ixx B1 = m B1 ∗ l 2 ∗ l 2 / 3
Izz B1 = 0.0
−− B3
m B3 = 1.0
r B3 = 0.01
Ixx B3 = m B3 ∗ J3 ∗ J3 / 3
Izz B3 = 0.0
67
−− B2
m B2 = 1.0
r B2 = 0.02
Ixx B2 = m B2 ∗ J3 ∗ J3 / 3
Izz B2 = 0.0
q1 = math . a c o s ( ( l 1 ˆ2 + l 2 ˆ2 − J3 ˆ 2 ) / ( 2 ∗ l 1 ∗ l 2 ) )
q2 = math . a c o s ( ( l 2 ∗ math . c o s ( q1 ) − l 1 ) / J3 )
bodies = {
virtual = {
mass = 0 ,
com = { 0 , 0 , 0 } ,
inertia = {
{0 , 0 , 0} ,
{0 , 0 , 0} ,
{0 , 0 , 0} ,
},
},
−− b a s e
base = {
mass = m base ,
com = { l 1 / 2 , 0 , 0 } ,
inertia = {
{ Ixx base , 0 , 0} ,
{0 , 0 , 0} ,
{0 , 0 , I z z b a s e } ,
},
},
−− B1
B1 = {
mass = m B1 ,
com = { l 2 / 2 , 0 , 0 } ,
inertia = {
{ Ixx B1 , 0 , 0 } ,
{0 , 0 , 0} ,
{0 , 0 , Izz B1 } ,
},
},
−− B2
B2 = {
68
mass = m B2 ,
com = { J3 / 2 , 0 , 0 } ,
inertia = {
{ Ixx B2 , 0 , 0 } ,
{0 , 0 , 0} ,
{0 , 0 , Izz B2 } ,
},
},
−− B3
B3 = {
mass = m B3 ,
com = { J3 / 2 , 0 , 0 } ,
inertia = {
{ Ixx B3 , 0 , 0 } ,
{0 , 0 , 0} ,
{0 , 0 , Izz B3 } ,
},
},
}
joints = {
revX = {
{1 , 0 , 0 , 0 , 0 , 0} ,
},
prsY = {
{0 , 0 , 0 , 0 , 1 , 0} ,
},
f i x e d = {} ,
}
meshes = {
base = {
name = ’ base ’ ,
dimensions = { l1 , r base , r base } ,
c o l o r = {0 , 0 , 1} ,
rotate = { axis = {0. , 0. , 1.} , angle = 90.0} ,
s r c = ’ u n i t c y l i n d e r m e d r e s z . obj ’ ,
mesh center = {0 , l 1 /2 , 0} ,
},
B1 = {
name = ’ B1 ’ ,
69
d i m e n s i o n s = { l 2 , r B1 , r B1 } ,
c o l o r = {1 , 0 , 0} ,
mesh center = {0 , l 2 /2 , 0} ,
},
B2 = {
name = ’ B2 ’ ,
d i m e n s i o n s = {J3 , r B2 , r B2 } ,
c o l o r = {0 , 1 , 0} ,
m e s h c e n t e r = { 0 , J3 / 2 , 0 } ,
},
B3 = {
name = ’ B3 ’ ,
d i m e n s i o n s = {J3 , r B3 , r B3 } ,
c o l o r = {0 , 1 , 1} ,
m e s h c e n t e r = { 0 , −J3 / 2 , 0 } ,
},
}
model = {
g r a v i t y = { 0 , −9.81 , 0 } ,
frames = {
−− b a s e
{
name = ’ base ’ ,
p a r e n t = ’ROOT’ ,
body = b o d i e s . base ,
joint = j o i n t s . fixed ,
v i s u a l s = { meshes . b a s e } ,
},
−− B1
{
name = ’ B1 ’ ,
p a r e n t = ’ base ’ ,
body = b o d i e s . B1 ,
j o i n t = j o i n t s . revX ,
70
joint frame = {
r = {0 , 0 , 0} ,
E = {
{1. , 0. , 0.} ,
{ 0 . , math . c o s ( q1 ) , math . s i n ( q1 ) } ,
{ 0 . , −math . s i n ( q1 ) , math . c o s ( q1 ) } ,
}
},
v i s u a l s = { meshes . B1 } ,
},
−− B2
{
name = ’ B2 ’ ,
p a r e n t = ’ base ’ ,
j o i n t = j o i n t s . revX ,
joint frame = {
r = {0 , l1 , 0} ,
E = {
{1. , 0. , 0.} ,
{ 0 . , math . c o s ( q2 ) , math . s i n ( q2 ) } ,
{ 0 . , −math . s i n ( q2 ) , math . c o s ( q2 ) } ,
}
},
},
−− B3
{
name = ’ B3 ’ ,
p a r e n t = ’ B2 ’ ,
j o i n t = j o i n t s . prsY ,
joint frame = {
r = { 0 , J3 , 0 } ,
},
},
},
−−C o n s t r a i n t s added
constraint sets = {
loop constraints = {
71
{
c o n s t r a i n t t y p e = ’ loop ’ ,
p r e d e c e s s o r b o d y = ’ B1 ’ ,
s u c c e s s o r b o d y = ’ B3 ’ ,
predecessor transform = {
E = {
{1 , 0 , 0} ,
{0 , 1 , 0} ,
{0 , 0 , 1} ,
},
r = {0 , l2 , 0} ,
},
successor transform = {
E = {
{1 , 0 , 0} ,
{0 , 1 , 0} ,
{0 , 0 , 1} ,
},
r = {0 , 0 , 0} ,
},
a x i s = {0 , 0 , 0 , 0 , 1 , 0} ,
s t a b i l i z a t i o n c o e f f i c i e n t = 0.02 ,
name = ’ loopTY ’ ,
},
{
c o n s t r a i n t t y p e = ’ loop ’ ,
p r e d e c e s s o r b o d y = ’ B1 ’ ,
s u c c e s s o r b o d y = ’ B3 ’ ,
predecessor transform = {
E = {
{1 , 0 , 0} ,
{0 , 1 , 0} ,
{0 , 0 , 1} ,
},
r = {0 , l2 , 0} ,
},
successor transform = {
E = {
{1 , 0 , 0} ,
{0 , 1 , 0} ,
{0 , 0 , 1} ,
72
},
r = {0 , 0 , 0} ,
},
a x i s = {0 , 0 , 0 , 0 , 0 , 1} ,
s t a b i l i z a t i o n c o e f f i c i e n t = 0.02 ,
name = ’ loopTZ ’ ,
},
},
},
}
r e t u r n model
After defining the model in LUA file, the model can be visualized using a meshup tool.
Figure A.1 visualizes the model.
Figure A.1: RRPR Model using Lua file
In Figure A.1, the blue link represents the base, red link represents body B1 , green link
represents stator part of the link B2 . The cut joint frame is at the end of the green link.
73
References
[1] Sebastian Bartsch et al. “Development and Control of the Multi-Legged Robot MAN-
TIS”. In: Proceedings of ISR 2016: 47st International Symposium on Robotics. 2016,
pp. 1–8 ( 4).
[2] J. Brinker and B. Corves. “A Survey on Parallel Robots with Delta-like Architecture”.
In: 2015 ( 2).
[3] Sébastien Briot and Wisama Khalil. Dynamics of Parallel Robots. Vol. 35. Sept. 2015.
isbn: 978-3-319-19787-6. doi: 10.1007/978-3-319-19788-3 ( 6).
[4] S. Delp et al. “OpenSim: Open-Source Software to Create and Analyze Dynamic
Simulations of Movement”. In: IEEE Transactions on Biomedical Engineering 54
(2007), pp. 1940–1950 ( 5).
[5] Johannes Englsberger et al. “Overview of the torque-controlled humanoid robot
TORO”. In: 2014 IEEE-RAS International Conference on Humanoid Robots. 2014,
pp. 916–923. doi: 10.1109/HUMANOIDS.2014.7041473 ( 3).
[6] R. Featherstone. Rigid Body Dynamics Algorithms. Springer, Boston, MA, 2008. isbn:
978-0-387-74314-1. doi: https://doi.org/10.1007/978- 1- 4899- 7560- 7 ( 6, 7,
9–11, 20, 27, 44–47).
[7] R. Featherstone and D. Orin. “Robot dynamics: equations and algorithms”. In: Pro-
ceedings 2000 ICRA. Millennium Conference. IEEE International Conference on
Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065). Vol. 1. 2000,
826–834 vol.1. doi: 10.1109/ROBOT.2000.844153 ( 44).
[8] M.L. Felis. “RBDL: an efficient rigid-body dynamics library using recursive algo-
rithms”. In: Auton Robot 41 (2017), pp. 495–511. doi: https://doi.org/10.1007/
s10514-016-9574-0 ( 5, 27, 36).
[9] Roberto Ierusalimschy. Programming in Lua, Fourth Edition. Lua.Org, 2016. isbn:
8590379868 ( 28).
74
[10] Abhinandan Jain. “Constraint Embedding”. In: Robot and Multibody Dynamics:
Analysis and Algorithms. Boston, MA: Springer US, 2011, pp. 297–311. isbn: 978-1-
4419-7267-5. doi: 10.1007/978-1-4419-7267-5_15. url: https://doi.org/10.
1007/978-1-4419-7267-5_15 ( 6, 7, 9, 27, 44, 45, 47, 49).
[11] W Khalil and E Dombre. “Chapter 1 - Terminology and general definitions”. In: Mod-
eling, Identification and Control of Robots. Ed. by W Khalil and E Dombre. Oxford:
Butterworth-Heinemann, 2002, pp. 1–12. isbn: 978-1-903996-66-9. doi: https : / /
doi.org/10.1016/B978-190399666-9/50001-4. url: https://www.sciencedirect.
com/science/article/pii/B9781903996669500014 ( 2).
[12] Daniel Kuehn et al. “System Design and Testing of the Hominid Robot Charlie”. In:
Journal of Field Robotics 34 (July 2016). doi: 10.1002/rob.21662 ( 4).
[13] Shivesh Kumar. “Modular and Analytical Methods for Solving Kinematics and Dy-
namics of Series-Parallel Hybrid Robots”. PhD thesis. Jan. 2019 ( 38, 39).
[14] Shivesh Kumar and Andreas Mueller. “An Analytical and Modular Software Work-
bench for Solving Kinematics and Dynamics of Series-Parallel Hybrid Robots”. In:
Aug. 2019. doi: 10.1115/DETC2019-97115 ( 5, 7, 27, 33, 35).
[15] Shivesh Kumar, CG Rajeevlochana, and Subir Kumar Saha. “Realistic Modeling
and Dynamic Simulation of KUKA KR5 Robot using RecurDyn”. In: 6th Asian
Conference on Multi-Body Dynamics (Aug. 2012) ( 55).
[16] Shivesh Kumar et al. “A survey on modularity and distributivity in series-parallel
hybrid robots”. In: Mechatronics 68 (2020), p. 102367. issn: 0957-4158. doi: https:
/ / doi . org / 10 . 1016 / j . mechatronics . 2020 . 102367. url: https : / / www .
sciencedirect.com/science/article/pii/S0957415820300477 ( 4).
[17] Shivesh Kumar et al. “An Analytical and Modular Software Workbench for Solving
Kinematics and Dynamics of Series-Parallel Hybrid Robots”. In: Journal of Mecha-
nisms and Robotics (JMR) 12.2 (Apr. 2020), pp. 1–12 ( 5).
[18] Shivesh Kumar et al. “Design and Kinematic Analysis of the Novel Almost Spherical
Parallel Mechanism Active Ankle”. In: Journal of Intelligent and Robotic Systems 94
(May 2019). doi: 10.1007/s10846-018-0792-x ( 4).
[19] Shivesh Kumar et al. “Modular Design and Decentralized Control of the Recupera
Exoskeleton for Stroke Rehabilitation”. In: Applied Sciences 9.4 (2019). issn: 2076-
3417. url: https://www.mdpi.com/2076-3417/9/4/626 ( 4).
[20] Kevin M. Lynch and Frank C. Park. Modern Robotics: Mechanics, Planning, and
Control. 1st. USA: Cambridge University Press, 2017. isbn: 1107156300 ( 2, 9, 11).
[21] Andreas Mueller. “Screw and Lie group theory in multibody dynamics”. In: Multibody
System Dynamics 42 (2018), pp. 219–248. doi: https://doi.org/10.1007/s11044-
017-9583-6 ( 14).
75
[22] A Müller. “Closed-form time derivatives of the equations of motion of rigid body
systems”. In: Multibody System Dynamics. 43 (2018), pp. 37–70. doi: https://doi.
org/10.1007/s11044-021-09796-8 ( 11).
[23] A. Müller and S. Kumar. “Screw and lie group theory in multibody kinematics”. In:
Multibody System Dynamics. (2021). doi: https://doi.org/10.1007/s11044-017-
9582-7 ( 9, 11).
[24] Richard M. Murray, S. Shankar Sastry, and Li Zexiang. A Mathematical Introduction
to Robotic Manipulation. 1st. USA: CRC Press, Inc., 1994. isbn: 0849379814 ( 9).
[25] S.K. Saha. Introduction to Robotics. Tata McGraw Hill Education Private Limited,
2008. isbn: 9781283439138. url: https : / / books . google . de / books ? id = %5C _
q4wngAACAAJ ( 2).
[26] Ajay Seth et al. “OpenSim: Simulating musculoskeletal dynamics and neuromuscular
control to study human and animal movement”. In: PLOS Computational Biology
14.7 (July 2018), pp. 1–20. doi: 10 .1371 / journal .pcbi . 1006223. url: https :
//doi.org/10.1371/journal.pcbi.1006223 ( 5).
[27] S.V. Shah, S.K. Saha, and J.K. Dutt. Dynamics of Tree-Type Robotic Systems. Intel-
ligent Systems, Control and Automation: Science and Engineering. Springer Nether-
lands, 2012. isbn: 9789400750067. url: https : / / books . google . de / books ? id =
4ygSl7MrY2oC ( 6).
[28] D. Stewart. “A Platform with Six Degrees of Freedom”. In: Proceedings of the In-
stitution of Mechanical Engineers 180.1 (1965), pp. 371–386. doi: 10.1243/PIME\
_PROC\_1965\_180\_029\_02 ( 2).
[29] Wikipedia. Parallel Manipulator. url: https://en.wikipedia.org/wiki/Parallel_
manipulator. (accessed: 01.09.2016) ( 2).
[30] Wikipedia. Serial Manipulator. url: https://en.wikipedia.org/wiki/Serial_
manipulator. (accessed: 01.09.2016) ( 2).
76
View publication stats

A Hybrid Numerical and Analytical Approach Towards Resolving Loop Closure Constraints in Rigid Body Dynamics

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

A Hybrid Numerical and Analytical Approach Towards Resolving Loop Closure Constraints in Rigid Body Dynamics

Uploaded by

Copyright:

Available Formats

See discussions, stats, and author profiles for this publication at: https://www.researchgate.

A hybrid numerical and analytical approach towards resolving loop closure

Thesis · August 2021

The user has requested enhancement of the downloaded file.

MASTER ERASMUS MUNDUS

Master Thesis Report

Rohit Kumar (Student Id: 5089482)

On August 27, 2021

A hybrid numerical and analytical approach towards

Supervisors : Shivesh Kumar (Senior Researcher, DFKI-RIC, Bremen)

Laboratory: Deutsches Forschungszentrum für Künstliche Intelligenz, Robotics Inno-

Secondly, I would like to express my gratitude towards my colleagues Shubham, Melya,

2 Modeling of Rigid Body systems 9

4 Implementation of Hybrid Numerical and Analytical Approach 33

5 Constraint Embedding for Forward Dynamics 44

6 Validation and Results 53

7 Conclusions and Future Work 65

A LUA File Extension for Defining the Model 67

1.1 Serial six degree of freedom manipulator [30] . . . . . . . . . . . . . . . . . 2

2.1 Lambda or RRPR mechanism . . . . . . . . . . . . . . . . . . . . . . . . . . 19

3.1 Topological graph of RRPR . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

4.1 Extraction of explicit constraints from the implicit constraints . . . . . . . . 34

5.1 Construction of new articulated body, adapted from [6] . . . . . . . . . . . 46

6.1 Spanning tree of the reduced version of RH5v2 upper body . . . . . . . . . 54

A.1 RRPR Model using Lua file . . . . . . . . . . . . . . . . . . . . . . . . . . . 73

1.1 Serial v/s Parallel robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.1 Screw coordinates for different joints . . . . . . . . . . . . . . . . . . . . . . 22

6.1 Computation time for analysis . . . . . . . . . . . . . . . . . . . . . . . . . 61

1 calc G from K(K) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

1.1.1 Different Topological Architectures in Robots

Figure 1.2: Parallel manipulator [29]

Figure 1.3: TORO [5] Humanoid robot

Table 1.1: Serial v/s Parallel robots

Serial robots Parallel robots

Series-Parallel Hybrid Robots:

(a) CHARLIE [12] robot (b) MANTIS [1] robot

(c) RECUPERA [19] ex-

Figure 1.4: Series-Parallel hybrid robots

Numerical Approach Analytical Approach

• Develop a combination of numerical and analytical formulations for resolving loop

• Implement in HyRoDyn software that has a modular framework for submechanisms.

• Test on a real hybrid robot to verify the robustness.

τ = M (q)q̈ + C(q, q̇) (1.1)

1.4 Articulate Body Algorithm with Constraint Embedding

• Although, analytical approach gives the better computational performance results

Modeling of Rigid Body systems

2.1 Graph Based Topology

2.1.1 Numbering Scheme

2.1.2 Different Properties of a Graph

2.2.1 Position Analysis

V si = V si−1 + AdTi,0 S i q̇i (2.12)

V i = AdTi,i−1 V i−1 + i X i q̇i (2.16)

2.2.3 Acceleration Analysis

The time derivative of adjoint transformation yields [21],

2.3 Mathematical Background on Constraint Equations of

3. Formulation of kinematic constraint equations due to the loop closure.

4. Combining the two formulated equations.

2.3.1 Implicit Constraints

where q is the vector of n generalized positions.

2.3.2 Explicit Constraints

2.3.3 Relationship between the Implicit and Explicit Form

where λ is a vector of nc unknown constraint force variable, also referred to as Lagrangian

Explicit form: Using the property of KG = 0, Lagrangian multipliers can be eliminated

The final equations of motion in independent coordinates(explicit form) can be written as