Assembly of the hypar structure with

Robot Arm
Robotics Course Project
AU University

Adam Kuryla
Akos Banfalvi
Isa B. I. Helal

January 2021
Contents
1 Introduction 4
1.1 Background and Motivation . . . . . . . . . . . . . . . . . . . 4
1.2 Objectives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.3 Outline of the report . . . . . . . . . . . . . . . . . . . . . . . 5

2 Problem formulation 6
2.1 Limitation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

3 The Model for Experimental Setup 7

3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
3.2 Sticks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
3.3 Stands for Assembly . . . . . . . . . . . . . . . . . . . . . . . 7
3.3.1 Hypar stand . . . . . . . . . . . . . . . . . . . . . . . . 7
3.3.2 Pickup stand for sticks . . . . . . . . . . . . . . . . . . 7
3.3.3 Pickup stand for joints . . . . . . . . . . . . . . . . . . 7
3.4 Joints . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
3.5 Robot Arm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
3.5.1 DH-parameters . . . . . . . . . . . . . . . . . . . . . . 10
3.6 Gripper . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
3.6.1 Development of fingers . . . . . . . . . . . . . . . . . . 12
3.7 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

4 Algorithm for Robot Arm 14

4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
4.2 Waypoints . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
4.3 Sequence . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
4.4 PolyScope algorithm . . . . . . . . . . . . . . . . . . . . . . . 15
4.4.1 TCP . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
4.4.2 Subprograms . . . . . . . . . . . . . . . . . . . . . . . 16
4.4.3 Main program . . . . . . . . . . . . . . . . . . . . . . . 17
4.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

5 Testing and Discussion 21

5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
5.2 Testing procedure . . . . . . . . . . . . . . . . . . . . . . . . . 21
5.3 Testing results and analysis . . . . . . . . . . . . . . . . . . . 21
5.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

1
6 Conclusion 22

A DH Parameters and transform matrix 24

B Main program for Polyscope 28

C Subprogram for PolyScope 33

2
List of Figures
1.1 Hypar Pavilon at Lincolm Center [1] . . . . . . . . . . . . . . 4

3.1 Sketch of Hypar [2] . . . . . . . . . . . . . . . . . . . . . . . . 8

3.2 Top view of Hypar . . . . . . . . . . . . . . . . . . . . . . . . 8
3.3 Picture of clamp joint . . . . . . . . . . . . . . . . . . . . . . . 9
3.4 Picture of magnetic joint . . . . . . . . . . . . . . . . . . . . . 9
3.5 Picture of UR5 Robot Arm . . . . . . . . . . . . . . . . . . . 10
3.6 DH frames for UR5 . . . . . . . . . . . . . . . . . . . . . . . . 10
3.7 DH parameters for UR5 . . . . . . . . . . . . . . . . . . . . . 11
3.8 Robotiq 2F-85 [3] . . . . . . . . . . . . . . . . . . . . . . . . . 12
3.9 Finger for Robotiq 2F-85 . . . . . . . . . . . . . . . . . . . . . 13

4.1 Placing joints . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

4.2 Placing sticks . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
4.3 First layer of sticks . . . . . . . . . . . . . . . . . . . . . . . . 15
4.4 Second layer of sticks . . . . . . . . . . . . . . . . . . . . . . . 15
4.5 TCP measurement . . . . . . . . . . . . . . . . . . . . . . . . 16
4.6 TCP coordinates . . . . . . . . . . . . . . . . . . . . . . . . . 16
4.7 Description of subprogram . . . . . . . . . . . . . . . . . . . . 17
4.8 Beginning of main program . . . . . . . . . . . . . . . . . . . 18
4.9 Code for placement of second layer . . . . . . . . . . . . . . . 19
4.10 Ending commands . . . . . . . . . . . . . . . . . . . . . . . . 19
4.11 Ending commands . . . . . . . . . . . . . . . . . . . . . . . . 20

3
1 | Introduction
1.1 Background and Motivation
Hypar Structure is a 3-dimensional surface, which is double-ruled. Structure
is becoming more and more popular in the architecture, where it is very often
used as a roof model as shown at the figure below.

Figure 1.1: Hypar Pavilon at Lincolm Center [1]

The growing demand for production of hypar structure requires new

methods of assembling it in faster, efficient and more secured way. The
most common existing method of doing it is a manual assembly, which takes
a lot of time due to complexity of the structure and effort to make it within
a tolerance, especially for mass production. Most of the existing projects
related to the structure skip descriptions of producing and assembling of it,
especially using modern autonomous methods.

1.2 Objectives
The main objective of the project is to automatize a production of the hypar
structure with the usage of the robot arm and to develop new techniques for
assembling it. Project has to apply a theory from Robotics Course.

4
1.3 Outline of the report
As a outline of the project it would be shown how the assembly can be done
with the usage of available robot arms and how structure could be done with
the usage of simple materials. In the experimental part, created components
and algorithms would be tested with a real robot, to present results and
define potential future improvements.

5
2 | Problem formulation
Project aims to find methods of assembling a hypar structure from small
branches and joints with a usage of UR robot. System must be tested by test-
ing/experiment made on physical model. List of sub-problems is established
as follows:

• Design, production, and implementation of physical model

– Development of joint connection

– Development of the suitable gripper for the robot
– Design and creation of the stand for the hypar structure
– Design and creation of the stand for sticks and joints

• Algorithm for the robot arm - with the usage of PolyScope

• Testing followed by validating and interpreting the data

• Identify potential problems

2.1 Limitation
Model is developed in the mini scale due to limitation of the workplace of the
robot and production methods. Robot is supposed to assembly the structure,
however further development and finishing of the model is not a concern of the
project. Some of the tasks can be limited due to problems with the tolerance
and accuracy, although algorithm is functioning, robot may not finished task
perfectly. Created program for robot is adjusted to the experimental setup
described in the project.Created set-up requires human interaction for placing
new components at stands and for adjusting small offsets.

6
3 | The Model for Experi-
mental Setup
3.1 Introduction
The real model of the project is very important, because it can reveal po-
tential problems with accuracy and design itself. What is more, based on
prototype model project can be improved and tested.

3.2 Sticks
3.3 Stands for Assembly
3.3.1 Hypar stand
3.3.2 Pickup stand for sticks
3.3.3 Pickup stand for joints

3.4 Joints
In order to connect sticks together appropriate joint method has to be de-
fined. During a process multiple ideas were defined and some of them tested.
The first step is to understand properties of the structure. Hypar structure
created of sticks as shown on the figures below does not make sticks to turn
around Z axis, therefore looking at it from the top all sticks are perpendicular
to each other. However angle of sticks in X and Y axis differs at every point
connection, that is why created joints must consider those changes. One of
the concern was if the distance between sticks is changing while the second
layer of sticks is placed perpendicularly at the top of the other. In theory the
distance is always the same at each point connection, however small offset
from tolerance in orientation can lead to problems with changing height. 3.2

7
 Figure 3.2: Top view of Hy-
Figure 3.1: Sketch of Hypar [2] par

Most of the ideas falls into three categories:

• Adhesive

• Clamping

• Magnetic

While adhesive is simple solution it requires multiple-tool gripper or modified

main gripper in order to apply for instance glue. Clamping ones are robust,
since they can hold structure infinitely, however from experience UR robot
struggled to attach them accurately due to flexibility of sticks and problems
with tolerance. Magnetic joints from the other hand are removing the prob-
lem with accuracy and adjustment of gripper fingers, however they can be
seen a temporary connection and at the end connections have to be improved
manually.
Models of two ideas were produced to confirm usability. The first of
them contains two clamps, which grab bottom and up stick together, the
cross connection in the middle allows joint to adjust angle according to angle
at specific position.3.3

8
 Figure 3.3: Picture of clamp joint

The second idea works on magnetic principles. One of the sticks has
attached magnet, while the second has magnetic joint which adjusts orienta-
tion and also the height in the case when there is a problem with accuracy
or angle change.3.4

Figure 3.4: Picture of magnetic joint

3.5 Robot Arm

Due to availability UR5 robot 3.5 was chosen to test principles of the projects.
UR is lightweight 6R industrial robot, which is capable of working up to

9
12.5kg of payload and has 1300mm of reach span. The main software, which
robot can be controlled with, is called PolyScope.

Figure 3.5: Picture of UR5 Robot Arm

3.5.1 DH-parameters
As mentioned before robot arm is built of 6 rotational joints and a DH
parameters and coordinates systems can be represented as below 3.6.

Figure 3.6: DH frames for UR5

10
 Figure 3.7: DH parameters for UR5

DH parameters can be then used to derive transform matrix (check Ap-

pendix:DH parameters and transform matrix A).

3.6 Gripper
Design of the gripper is another concern, which has to be taken into account,
since grippers very often are flexible in modification and designing new grip-
per is complex and expensive, therefore only fingers can be modified in order
to satisfy project’s needs. For the project grasping gripper was used, which
is called Robotiq 2F-85. Gripper has two fingers.

11
 Figure 3.8: Robotiq 2F-85 [3]

3.6.1 Development of fingers

The simplest method in adjusting the gripper is to change its fingers so that
it could handle materials used in the project.

• Specifications:

– fingers has to handle round stick (usage of round or V groove)

– fingers should handle joints (depending on joint geometry, it can
be either flat surface or finger with groove)

Since, it is important that sticks stays perpendicular during the grip, V or

round grooves must be done. Therefore, the final design of fingers for gripper
is presented as below:

12
 Figure 3.9: Finger for Robotiq 2F-85

Since there is round groove robot would stabilize the joint while taking it
by the head, restricting it from rotation. Moreover, sticks are round, that is
why the shape would fix them in the perpendicular position to the gripper.
Groove has the diameter of 8mm with tolerance +/-2mm.

3.7 Summary
The idea of assembling the hypar structure with usage of robot was so far
treated as theoretical model. Concept of developing appropriate components
for testing is demand, since it can prove the usability of the robot. The
joint design was the biggest concern, since both joints designs could be used,
however the first idea (clamps) was chosen since it is the only one that would
make the assembly stable after the procedure and can be more likely be
used in production scale. Magnetic joints are easier to use, however they
make the whole procedure of preparing setup for them much more complex
and time deficient, since they require manual interaction after assembly and
preparation of sticks before assembling.

13
4 | Algorithm for Robot Arm
4.1 Introduction
The next step in the project is to establish the appropriate code for the robot,
so that it could assembly the structure effortlessly. In order to make it, first
waypoints have to established from defined dimensions and then the code
with waypoints has to created in PolyScope.

4.2 Waypoints
4.3 Sequence
Once waypoints are established it is convenient to define the skeleton of the
algorithm, in the other words,to set sequence of tasks for the robot arm. It is
important to mention that the sequence can differ according to experimental
setup. In this case, algorithm is adjusted to the experimental layout that
was described in previous chapters.
The whole sequence starts with robot putting joints into the proper po-
sition of joints (taking joints from stand for joints), one by one for each stick
(stick standing at stand for sticks). While joints are placed at a stick, then
stick will be immediately moved to the position at the hypar stand.4.3

Figure 4.1: Placing joints Figure 4.2: Placing sticks

At this moment sticks are placed with joints and the first layer.4.3

14
 Figure 4.3: First layer of sticks

The last step is to place the top layer of sticks at the top of previous one.
4.4

Figure 4.4: Second layer of sticks

At this point the whole procedure is finished. 4.4

4.4 PolyScope algorithm

When the sequence is established, the main programming part can be con-
ducted.

15
4.4.1 TCP
At the beginning of the program TCP point is measured and set up in the set-
tings, so that robot would move properly the gripper to calclulated positions
of waypoints. 4.4.1

Figure 4.5: TCP measurement Figure 4.6: TCP coordinates

4.4.2 Subprograms
Main program in PolyScope except main procedures includes also subpro-
grams, which makes the coding faster and can help to skip copying and
pasting the same code over and over. There are three subprograms:

• Subprogram A - Placing joints at the stick, which is at the A position

on stand for sticks.

• Subprogram B - Placing joints at the stick, which is at the B position

on stand for sticks.

• Subprogram C - Placing joints at the stick, which is at the C position

on stand for sticks. Procedure then is conducted as described at the
figure below 4.7.

16
 Figure 4.7: Description of subprogram

Type L of movement is chosen since gripper has to go linearly from up

towards the points that joints are placed.

4.4.3 Main program

Main program includes all of the commands for moving around sticks and
also subprograms, which are responsible for placement of joints on sticks.
Program at the beginning is activating the gripper and goes to the safe po-
sition (initial). After that it starts the whole procedure of assembling. First
five sticks (P1-P5) except of placement at the correct position is subjected
to the subprograms depending on the placement on the hypar stand. 4.8

17
 Figure 4.8: Beginning of main program

The rest of sticks (Q1-Q5) is placed as a second layer at the top of previous
ones in the selected positions. The procedure is almost the same as that
described before, however in this case subprograms for assembling joints are
skipped 4.9.

18
 Figure 4.9: Code for placement of second layer

When the whole assembly is done, robot is going back for initial position
and robot becomes deactivated with a halt command. 4.10

Figure 4.10: Ending commands

In some cases due to obstacles or torque some of waypoints had to be

reached with different orientation of joints, therefore inverse kinematics had
to be applied and to simplify procedure RoKiSim software was used to gen-
erate values for joints, which then were used in the program.4.11

19
 Figure 4.11: Ending commands

4.5 Summary
The whole algorithm can be seen as a robust for simple tasks and it is full
filling all needs for experimental setup. The whole program can be seen in
the attachments.B C

20
5 | Testing and Discussion
5.1 Introduction
5.2 Testing procedure
5.3 Testing results and analysis
5.4 Summary

21
6 | Conclusion

22
Bibliography
[1] Inhabitat. Hypar Pavilion at Lincoln Center. https://inhabitat.com/
lincoln-centers-green-roofed-restaurant-now-serving/
hypar-pavilion-at-lincoln-center-10/, 2011. Downloaded:
16-01-2022.

[2] Sarmila Sahoo. Hypar sketch.

https://www.researchgate.net/publication/258379277/figure/
fig1/AS:307387459227658@1450298289318/
Surface-of-a-hypar-shell-with-cutout-Surface-equation-z4c-abx-a-2y-b-2.
png, 2011. Downloaded: 12-01-2022.

[3] WiredWorkers. Robotiq 2F-85.

https://wiredworkers.io/product/robitiq-2f-85/, 2022.
Downloaded: 16-01-2022.

23
A | DH Parameters and trans-
form matrix
Derivation of transform matrix from DH parameters.

24
Calculus for Lab4

1.Derivation of T06

⎡ cos ((θ)) -cos ((α)) ⋅ sin ((θ)) sin ((α)) ⋅ sin ((θ)) a ⋅ cos ((θ)) ⎤
⎢ sin ((θ)) cos ((α)) ⋅ cos ((θ)) -sin ((α)) ⋅ cos ((θ)) a ⋅ sin ((θ)) ⎥
DHmatrix ((α , a , d , θ)) ≔ ⎢ ⎥
⎢ 0 sin ((α)) cos ((α)) d ⎥
⎣ 0 0 0 1 ⎦
For each joint:
⎡ cos ((θ1)) 0 sin ((θ1)) 0 ⎤
⎛π ⎞ ⎢ sin ((θ1)) 0 -cos ((θ1)) 0 ⎥
T01 ≔ DHmatrix ⎜―, 0 , 0.089159 , θ1⎟ → ⎢ ⎥
⎝2 ⎠ ⎢ 0 1 0 0.089159 ⎥
⎣ 0 0 0 1 ⎦

⎡ cos ((θ2)) -sin ((θ2)) 0 -0.425 ⋅ cos ((θ2)) ⎤

⎢ sin ((θ2)) cos ((θ2)) 0 -0.425 ⋅ sin ((θ2)) ⎥
( )
T12 ≔ DHmatrix (0 , -0.425 , 0 , θ2) → ⎢ ⎥
⎢ 0 0 1 0 ⎥
⎣ 0 0 0 1 ⎦

⎡ cos ((θ3)) -sin ((θ3)) 0 -0.39225 ⋅ cos ((θ3)) ⎤

⎢ sin ((θ3)) cos ((θ3)) 0 -0.39225 ⋅ sin ((θ3)) ⎥
T23 ≔ DHmatrix ((0 , -0.39225 , 0 , θ3)) → ⎢ ⎥
⎢ 0 0 1 0 ⎥
⎣ 0 0 0 1 ⎦
⎡ cos ((θ4)) 0 sin ((θ4)) 0 ⎤
⎛π ⎞ ⎢ sin ((θ4)) 0 -cos ((θ4)) 0 ⎥
T34 ≔ DHmatrix ⎜―, 0 , 0.10915 , θ4⎟ → ⎢ ⎥
⎝2 ⎠ ⎢ 0 1 0 0.10915 ⎥
⎣ 0 0 0 1 ⎦

Created with PTC Mathcad Express. See www.mathcad.com for more information.
 ⎡ cos ((θ4)) 0 sin ((θ4)) 0 ⎤
⎛π ⎞ ⎢ sin ((θ4)) 0 -cos ((θ4)) 0 ⎥
T34 ≔ DHmatrix ⎜―, 0 , 0.10915 , θ4⎟ → ⎢ ⎥
⎝2 ⎠ ⎢ 0 1 0 0.10915 ⎥
⎣ 0 0 0 1 ⎦

⎡ cos ((θ5)) 0 -sin ((θ5)) 0 ⎤

⎛ π ⎞ ⎢ sin ((θ5)) 0 cos ((θ5)) 0 ⎥
T45 ≔ DHmatrix ⎜-―, 0 , 0.09465 , θ5⎟ → ⎢ ⎥
⎝ 2 ⎠ ⎢ 0 -1 0 0.09465 ⎥
⎣ 0 0 0 1 ⎦

⎡ cos ((θ6)) -sin ((θ6)) 0 0 ⎤

⎢ sin ((θ6)) cos ((θ6)) 0 0 ⎥
T56 ≔ DHmatrix ((0 , 0 , 0.0823 , θ6)) → ⎢ ⎥
⎢ 0 0 1 0.0823 ⎥
⎣ 0 0 0 1 ⎦

Tranform 0->6 (Matrix is too large to display even on the A3, therefore the main formula is
cut and each column is represented)

⎡ cos ((θ2 + θ3)) ⋅ cos ((θ1)) -((sin ((θ2 + θ3)) ⋅ cos ((θ1)))) sin ((θ1)) -((0.196125 ⋅ cos ((θ1 + θ2 + θ3)))) - 0.2125 ⋅ cos ((θ1 - 1
simplify ⎢ cos ((θ2 + θ3)) ⋅ sin ((θ1)) -((sin ((θ2 + θ3)) ⋅ sin ((θ1)))) -cos ((θ1)) -((0.196125 ⋅ sin ((θ1 + θ2 + θ3)))) - 0.2125 ⋅ sin ((θ1 - 1
T03 ≔ T01 ⋅ T12 ⋅ T23 ――― →⎢ ...
⎢ sin ((θ2 + θ3)) cos ((θ2 + θ3)) 0 0.089159 - 0.425
⎣ 0 0 0

⎡ cos ((θ2 + θ3 + θ4)) ⋅ cos ((θ1)) sin ((θ1)) sin ((θ2 + θ3 + θ4)) ⋅ cos ((θ1)) 0.10915 ⋅ sin ((θ1)) - 0.2125 ⋅ cos ((θ1 - 1.0 ⋅ θ2)) - 0.2
simplify ⎢ cos ((θ2 + θ3 + θ4)) ⋅ sin ((θ1)) -cos ((θ1)) sin ((θ2 + θ3 + θ4)) ⋅ sin ((θ1)) -((0.196125 ⋅ sin ((θ1 + θ2 + θ3)))) - 0.2125 ⋅ sin ((θ1 - 1
T04 ≔ T03 ⋅ T34 ――― →⎢ ...
⎢ sin ((θ2 + θ3 + θ4)) 0 -cos ((θ2 + θ3 + θ4)) 0.0891
⎣ 0 0 0
⎡ cos ((θ6)) ⋅ sin ((θ1)) ⋅ sin ((θ5)) - sin ((θ2 + θ3 + θ4)) ⋅ cos ((θ1)) ⋅ sin ((θ6)) + cos ((θ2 + θ3 + θ4)) ⋅ cos ((θ1)) ⋅ cos ((θ5)) ⋅ cos ((θ6))
simplify ⎢ cos ((θ2 + θ3 + θ4)) ⋅ cos ((θ5)) ⋅ cos ((θ6)) ⋅ sin ((θ1)) - sin ((θ2 + θ3 + θ4)) ⋅ sin ((θ1)) ⋅ sin ((θ6)) - cos ((θ1)) ⋅ cos ((θ6)) ⋅ sin ((θ5))
T06 ≔ T04 ⋅ T45 ⋅ T56 ――― →⎢
⎢ cos ((θ2 + θ3 + θ4)) ⋅ sin ((θ6)) + sin ((θ2 + θ3 + θ4)) ⋅ cos ((θ5)) ⋅ cos ((θ6))
⎣ 0

Representation by columns

Created with PTC Mathcad Express. See www.mathcad.com for more information.
Representation by columns

⎡ cos ((θ6)) ⋅ sin ((θ1)) ⋅ sin ((θ5)) - sin ((θ2 + θ3 + θ4)) ⋅ cos ((θ1)) ⋅ sin ((θ6)) + cos ((θ2 + θ3 + θ4)) ⋅ cos ((θ1)) ⋅ cos ((θ5)) ⋅ cos ((θ6)) ⎤
⎢ cos (θ2 + θ3 + θ4) ⋅ cos ( ) ⋅ cos ( ) ⋅ sin ( ) - sin (θ2 + θ3 + θ4) ⋅ sin ( ) ⋅ sin ( ) - cos ( ) ⋅ cos ( ) ⋅ sin ( ) ⎥
( ) (θ5) (θ6) (θ1) ( ) (θ1) (θ6) (θ1) (θ6) (θ5)
T06$1 ≔ submatrix ((T06 , 0 , 3 , 0 , 0)) ―
→ ⎢ ⎥
⎢ cos ((θ2 + θ3 + θ4)) ⋅ sin ((θ6)) + sin ((θ2 + θ3 + θ4)) ⋅ cos ((θ5)) ⋅ cos ((θ6)) ⎥
⎢⎣ 0 ⎥⎦

⎡ -((sin ((θ1)) ⋅ sin ((θ5)) ⋅ sin ((θ6)))) - sin ((θ2 + θ3 + θ4)) ⋅ cos ((θ1)) ⋅ cos ((θ6)) - cos ((θ2 + θ3 + θ4)) ⋅ cos ((θ1)) ⋅ cos ((θ5)) ⋅ sin ((θ6)) ⎤
⎢ cos ( ) ⋅ sin ( ) ⋅ sin ( ) - sin (θ2 + θ3 + θ4) ⋅ cos ( ) ⋅ sin ( ) - cos (θ2 + θ3 + θ4) ⋅ cos ( ) ⋅ sin ( ) ⋅ sin ( ) ⎥
(θ1) (θ5) (θ6) ( ) (θ6) (θ1) ( ) (θ5) (θ1) (θ6)
T06$2 ≔ submatrix ((T06 , 0 , 3 , 1 , 1)) ―
→ ⎢ ⎥
⎢ cos ((θ2 + θ3 + θ4)) ⋅ cos ((θ6)) - sin ((θ2 + θ3 + θ4)) ⋅ cos ((θ5)) ⋅ sin ((θ6)) ⎥
⎢⎣ 0 ⎥⎦

⎡ cos ((θ5)) ⋅ sin ((θ1)) - cos ((θ2 + θ3 + θ4)) ⋅ cos ((θ1)) ⋅ sin ((θ5)) ⎤
⎢ -(cos ( ) ⋅ cos ( )) - cos (θ2 + θ3 + θ4) ⋅ sin ( ) ⋅ sin ( ) ⎥
( (θ1) (θ5)) ( ) (θ1) (θ5)
T06$3 ≔ submatrix ((T06 , 0 , 3 , 2 , 2)) ―
→ ⎢ ⎥
⎢ -((sin ((θ2 + θ3 + θ4)) ⋅ sin ((θ5)))) ⎥
⎢⎣ 0 ⎥⎦

⎡ -0.196125 ⋅ cos ((θ1 - 1.0 ⋅ θ2 - 1.0 ⋅ θ3)) + 0.10915 ⋅ sin ((θ1)) - 0.196125 ⋅ cos ((θ1 + θ2 + θ3)) - 0.2125 ⋅ cos ((θ1 - 1.0 ⋅ θ2)) - 0.2125 ⋅ cos ((θ1 + θ2)) + 0.0823 ⋅ cos ((θ5)) ⋅ sin ((θ1)) + 0.09465 ⋅ sin ((θ2 + θ3 + θ4)) ⋅ cos ((θ1)) - 0.0823 ⋅ cos ((θ2 + θ3 + θ4)) ⋅ cos ((θ1)) ⋅ sin ((θ5)) ⎤
⎢ -0.196125 ⋅ sin (θ1 - 1.0 ⋅ θ2 - 1.0 ⋅ θ3) - 0.10915 ⋅ cos ( ) - 0.196125 ⋅ sin (θ1 + θ2 + θ3) - 0.2125 ⋅ sin (θ1 - 1.0 ⋅ θ2) - 0.2125 ⋅ sin (θ1 + θ2) - 0.0823 ⋅ cos ( ) ⋅ cos ( ) + 0.09465 ⋅ sin (θ2 + θ3 + θ4) ⋅ sin ( ) - 0.0823 ⋅ cos (θ2 + θ3 + θ4) ⋅ sin ( ) ⋅ sin ( ) ⎥
( ) (θ1) ( ) ( ) ( ) (θ1) (θ5) ( ) (θ1) ( ) (θ1) (θ5)
T06$4 ≔ submatrix ((T06 , 0 , 3 , 3 , 3)) ―
→ ⎢ ⎥
⎢ -0.09465 ⋅ cos ((θ2 + θ3 + θ4)) - 0.04115 ⋅ cos ((θ2 + θ3 + θ4 - 1.0 ⋅ θ5)) + 0.04115 ⋅ cos ((θ2 + θ3 + θ4 + θ5)) - 0.39225 ⋅ sin ((θ2 + θ3)) - 0.425 ⋅ sin ((θ2)) + 0.089159 ⎥
⎢⎣ 1 ⎥⎦

Porg06 is represented by 3 first rows of T06$4.

2.Position vector Porg05

While having the tranform matrix from 0 to 5, we easily define Porg05.

⎡ cos ((θ5)) ⋅ ((cos ((θ2 + θ3)) ⋅ cos ((θ1)) ⋅ cos ((θ4)) - sin ((θ2 + θ3)) ⋅ cos ((θ1)) ⋅ sin ((θ4)))) + sin ((θ1)) ⋅ sin ((θ5)) -((cos ((θ2 + θ3)) ⋅ cos (θ1
( )) ⋅ sin ((θ4)))) - sin ((θ2 + θ3)) ⋅ cos ((θ1)) ⋅ cos ((θ4)) cos ((θ5)) ⋅ sin ((θ1)) - sin ((θ5)) ⋅ ((cos ((θ2 + θ3)) ⋅ cos ((θ1)) ⋅ cos ((θ4)) - sin ((θ2 + θ3)) ⋅ cos ((θ1)) ⋅ sin ((θ4)))) -0.196125 ⋅ cos ((θ1 - 1.0 ⋅ θ2 - 1.0 ⋅ θ3)) + 0.10915 ⋅
⎢ cos ( ) ⋅ (cos (θ2 + θ3) ⋅ cos ( ) ⋅ sin ( ) - sin (θ2 + θ3) ⋅ sin ( ) ⋅ sin ( )) - cos ( ) ⋅ sin ( ) -((cos ((θ2 + θ3)) ⋅ sin ((θ1)) ⋅ sin ((θ4)))) - sin ((θ2 + θ3)) ⋅ cos ((θ4)) ⋅ sin ((θ1)) -((sin ((θ5)) ⋅ ((cos ((θ2 + θ3)) ⋅ cos ((θ4)) ⋅ sin ((θ1)) - sin ((θ2 + θ3)) ⋅ sin ((θ1)) ⋅ sin ((θ4)))))) - cos ((θ1)) ⋅ cos ((θ5)) -0.196125 ⋅ sin ((θ1 - 1.0 ⋅ θ2 - 1.0 ⋅ θ3)) - 0.10915
T05 ≔ T03 ⋅ T34 ⋅ T45 ―→ ⎢
⎢
(θ5) ( ( ) (θ4) (θ1) ( ) (θ1) (θ4))
cos ((θ5)) ⋅ ((cos ((θ2 + θ3)) ⋅ sin ((θ4)) + sin ((θ2 + θ3)) ⋅ cos ((θ4))))
(θ1) (θ5)
cos ((θ2 + θ3)) ⋅ cos ((θ4)) - sin ((θ2 + θ3)) ⋅ sin ((θ4)) -((sin ((θ5)) ⋅ ((cos ((θ2 + θ3)) ⋅ sin ((θ4)) + sin ((θ2 + θ3)) ⋅ cos ((θ4)))))) ...
⎢⎣ 0 0 0

Matrix is too big to display, hovewer using the function of submatrix in MathCad we can represent Porg05.

Porg ((T)) ≔ submatrix ((T , 0 , 2 , 3 , 3))

⎡ -0.196125 ⋅ cos ((θ1 - 1.0 ⋅ θ2 - 1.0 ⋅ θ3)) + 0.10915 ⋅ sin ((θ1)) - 0.196125 ⋅ cos ((θ1 + θ2 + θ3)) - 0.2125 ⋅ cos ((θ1 - 1.0 ⋅ θ2)) - 0.2125 ⋅ cos ((θ1 + θ2)) + 0.09465 ⋅ cos ((θ2 + θ3)) ⋅ cos ((θ1)) ⋅ sin ((θ4)) + 0.09465 ⋅ sin ((θ2 + θ3)) ⋅ cos ((θ1)) ⋅ cos ((θ4)) ⎤
→ ⎢ -0.196125 ⋅ sin ((θ1 - 1.0 ⋅ θ2 - 1.0 ⋅ θ3)) - 0.10915 ⋅ cos ((θ1)) - 0.196125 ⋅ sin ((θ1 + θ2 + θ3)) - 0.2125 ⋅ sin ((θ1 - 1.0 ⋅ θ2)) - 0.2125 ⋅ sin ((θ1 + θ2)) + 0.09465 ⋅ cos ((θ2 + θ3)) ⋅ sin ((θ1)) ⋅ sin ((θ4)) + 0.09465 ⋅ sin ((θ2 + θ3)) ⋅ cos ((θ4)) ⋅ sin ((θ1))
Porg ((T05)) ― ⎥
⎢ ⎥
⎣ -0.39225 ⋅ sin ((θ2 + θ3)) - 0.425 ⋅ sin ((θ2)) - 0.09465 ⋅ cos ((θ2 + θ3)) ⋅ cos ((θ4)) + 0.09465 ⋅ sin ((θ2 + θ3)) ⋅ sin ((θ4)) + 0.089159 ⎦

3.Impact of change of variables on the y coordinate of position vector from 0 to 6

Created with PTC Mathcad Express. See www.mathcad.com for more information.
B | Main program for Polyscope
Representation of the main program for PolyScope

28
Main program for PolyScope
C | Subprogram for PolyScope

33
Subprogram

Subprograms for A, B and C are similar, they are different only with different waypoints position and naming (f.ex a1 can
be b1 or c1 etc.)