Solutions - Assignment 1

1) SE(2), T
3
, T
5
, SE(2)T
5
, (SE(2)T
5
)
2

2)
Utility Functions

Distance(a
x
,a
y
,b
x
,b
y
) //calculate distance between a and b
return ((a
x
-b
x
)
2
+(a
y
-b
y
)
2
)
1/2


AngleDifference(
1
,
2
)
// this function entirely depends upon whether you prefer angles from - to or from 0 to 2
//based on the choice please reduce both
1
and
2
to the same range and see if clockwise difference is
//lesser or the anti-clockwise. There is a better implementation, which I'll test and provide.

max(a,b) if ab, return a, else return b

min(a,b) if ab, return a, else return b

Line(a
x
,a
y
,b
x
,b
y
) //checks if line from point a to b is collision-free
=tan
-1
((b
y
-a
y
) / (b
x
-a
x
)) //TIP: Use atan2
for i=0 to distance(a
x
,a
y
,b
x
,b
y
) in steps
if W([a
x
a
y
]+i.[cos() sin()]) return true
return false

Circle(a
x
, a
y
, R) //checks if a circle centered at a with radius R is collision-free
for = - to in steps
if W([a
x
, a
y
]+R.[cos() sin()]) return true
return false

Disk(a
x
, a
y
, R) //checks if a circular disk centered at a with radius R is collision-free
for r=1 to R in steps
if Circle(a
x
, a
y
,r) return true
return false

Corners(a
x
, a
y
, , l, w) //returns corners of a rectangle centered at a, length l, width w, orientation
= tan
-1
( (w/2) / (l/2) ) // or simply tan
-1
(w/l)
d=(l
2
+w
2
)
1/2
/ 2
C
1
=[a
x
a
y
] + d.[cos(+) sin(+)]
C
2
=[a
x
a
y
] + d.[cos(-) sin(-)]
C
3
=[a
x
a
y
] - d.[cos(+) sin(+)]
C
4
=[a
x
a
y
] - d.[cos(-) sin(-)]
return C
1
, C
2
, C
3
, C
4


Rectangle(a
x
, a
y
, , l, w) //checks if a rectangle centered at a
// length l , width w, orientation , is collision-free
= tan
-1
( (C
4
y-C
3
y) / (C
4
x-C
3
x) )
//hint use atan2, can also be obtained from and
C
1
, C
2
, C
3
, C
4
= corners(a
x
, a
y
, , l, w)
for i=0 to w in steps
P = C
3
+i.[cos() sin()]
Q = C
2
+i.[cos() sin()]
if line(P,Q) return true
return false

X
Y

a

l
w
d
C
1

C
2

C
3

C
4

X
Y
a
l
C
1

C
2

C
3

C
4

w
i
P
Q
Distance3D(a
x
,a
y
,a
z
,b
x
,b
y
, b
z
) //calculate distance between a and b
return ((a
x
-b
x
)
2
+(a
y
-b
y
)
2
+(a
z
-b
z
)
2
)
1/2


Line3D(a
x
,a
y
,a
z
,b
x
,b
y
,b
z
)
D=distance3D(a
x
,a
y
,a
z
,b
x
,b
y
,b
z
)
for i=0 to D in steps
p=[a
x
a
y
a
z
]+i.[(b
x
- a
x
)/D (b
y
- a
y
)/D (b
z
- a
z
)/D]
if W(p
x
,p
y
,p
z
) return true
return false

Ring3D(a
x
, a
y
, a
z
, R, h) //additionally, height h. assumed that the principle axis is Z
for i=a
z
-h/2 to a
z
+h/2 in steps
for = - to in steps
if W([a
x
, a
y
i]+R.[cos() sin() 0]) return true
return false

Disk3D(a
x
, a
y
, a
z
, h, R) //additionally height h
for r=1 to R in steps
if Ring3D(a
x
, a
y
, a
z
, r, h) return true
return false

Sphere3D(a
x
, a
y
, a
z
, R)
for =0 to in steps
if Disk3D(a
x
, a
y
, a
z
+R.Cos(), 0, R.Sin()) return true
return false


2-i) Collide(
1
,
2,

3
)
let a be the location of the fixed base
b = [a
x
a
y
]+l
1
.[cos(
1
) sin(
1
)] //end of link 1
c = [b
x
b
y
]+l
2
.[cos(
1
+
2
) sin(
1
+
2
)] //end of link 2
d = [c
x
c
y
]+l
3
.[cos(
1
+
2
+
3
) sin(
1
+
2
+
3
)] //end of link 3
return line(a,b) or line(b,c) or line(c,d)

2-ii) Collide(
1
,
2,

3
,
4
,
5
) //note that this solution assumes that the fingers
//are 'not' coupled and are independent. People who have assumed coupled fingers
// should take
5
= -
4
or any other coupling relation
let a be the location of the fixed base
b = [a
x
a
y
]+l
1
.[cos(
1
) sin(
1
)] //end of link 1
c = [b
x
b
y
]+l
2
.[cos(
1
+
2
) sin(
1
+
2
)] //end of link 2
d = [c
x
c
y
]+l
3
.[cos(
1
+
2
+
3
) sin(
1
+
2
+
3
)] //end of link 3
e = [d
x
d
y
]+f
1
.[cos(
1
+
2
+
3
+
4
) sin(
1
+
2
+
3
+
4
)] //end of finger 1 of length f
1

f = [d
x
d
y
]+f
2
.[cos(
1
+
2
+
3
+
5
) sin(
1
+
2
+
3
+
5
)] //end of finger 1 of length f
2

return line(a,b) or line(b,c) or line(c,d) or line (d,e) or line(d,f)

2-iii) Collide(a
x
, a
y
) return disk(a
x
,a
y
,R)

2-iv) Collide(a
x
, a
y
, ,
1
,
2,

3
,
4
,
5
) //Note that in this question the workspace is 3D and NOT 2D as in the
previous cases. Hence W(x,y) notation should from now be changed to W(x,y,z).
Further note that in question 2-ii a vertical plane was used and called as the X-Y plane. Now however there are two
planes: one where the circular disk robot operates, which will now be called as the XY plane, and another one where
the manipulator operates. We will first assume that the manipulator operates in a strict X-Z plane. Then we will
rotate the circular disk robot by angle along the Z axis, which will rotate the manipulator across the Z axis.
if Disk3D(a
x
, a
y
, h, R) return true
b = [a
x
a
y
0]+l
1
.[cos(
1
) 0 sin(
1
)] //end of link 1
c = [b
x
b
y
b
z
]+l
2
.[cos(
1
+
2
) 0 sin(
1
+
2
)] //end of link 2
l
1

l
2

l
3

a
b
c
d
f
1

f
2

e
f

4

-
5

R
p=R.Cos()

r=R.Sin()
a
d = [c
x
c
y
c
z
]+l
3
.[cos(
1
+
2
+
3
) 0 sin(
1
+
2
+
3
)] //end of link 3
e = [d
x
d
y
d
z
]+f
1
.[cos(
1
+
2
+
3
+
4
) 0 sin(
1
+
2
+
3
+
4
)] //end of finger 1 of length f
1

f = [d
x
d
y
d
z
]+f
2
.[cos(
1
+
2
+
3
+
5
) 0 sin(
1
+
2
+
3
+
5
)] //end of finger 1 of length f
2
b=MAGIC(b,), c=MAGIC(c,), d=MAGIC(d,), e=MAGIC(e,), f=MAGIC(f,)
//Some magic function rotates the points by the needed . Out of syllabus (Rotation Matrix)
return line3D(a,b) and line3D(b,c) and line3D(c,d) and line3D(d,e) and line3D(d,f)

2-v) The question entirely depends upon the manner by which the disk is chosen. Most practical cases involve
the use of rotational matrices, which is not taken as a part of this course, and hence the question was audited.

2-vi) Collide (a
x
,a
y
,b
x
,b
y
) return line(a
x
,a
y
,b
x
,b
y
)

2-vii) Collide (a
x
,a
y
,b
x
,b
y
)
=tan
-1
((b
y
-a
y
) / (b
x
-a
x
)) //TIP: Use atan2
for i=0 to distance(a
x
,a
y
,b
x
,b
y
) in steps //lets iterate i over the line, and at each step we'll place a disk
p=[a
x
,a
y
]

+ i.[cos() sin()] //position of disk on its way from a to b
if disk(p
x
, p
y
,R) return true
return false

2-viii) Collide (a
x
,a
y
,b
x
,b
y
,

c
x
,c
y
,d
x
,d
y
)

1
=tan
-1
((b
y
-a
y
) / (b
x
-a
x
)) //angle of motion of disk 1

2
=tan
-1
((b
y
-a
y
) / (b
x
-a
x
)) //angle of motion of disk 2
for i=0 to max( distance(a
x
,a
y
,b
x
,b
y
) , distance(c
x
,c
y
,d
x
,d
y
) ) in steps //lets simultaneously iterate across
//two completely different lines. Since speeds are same, at any instant of time in their motions,
//the two disks would have travelled the same distance i.
d
1
= min(i, distance(a
x
,a
y
,b
x
,b
y
)) //distance moved by disk 1
d
2
= min(i, distance(c
x
,c
y
,d
x
,d
y
)) //distance moved by disk 2
p
1
= [a
x
,a
y
]

+ d
1
.[cos(
1
) sin(
1
)] //position of disk 1 on its way from a to b
p
2
= [c
x
,c
y
]

+ d
2
.[cos(
2
) sin(
2
)] //position of disk 2 on its way from c to d
if disk(p
1x
, p
1y
,R
1
) or disk(p
2x
, p
2y
,R
2
) or distance(p
1
,p
2
)R
1
+R
2
//two disks placed at p1 and p2
//collide if the distance between them is less than sum of their radii
return true
return false

2-ix) Collide (a
x
,a
y
,a

,b
x
,b
y
, b

)
//Correction in question: The robot was supposed to be holonomic.
//First try to move the rectangle from a to b discarding angles.
//Then make rotations on the way
=tan
-1
((b
y
-a
y
) / (b
x
-a
x
)) //TIP: Use atan2
D = distance(a
x
,a
y
,b
x
,b
y
)
for i=0 to D in steps //lets iterate i over the line, and at each step we'll place a rectangle
p=[a
x
,a
y
]

+ i.[cos() sin()] //position of disk on its way from a to b
p

=a

+ i.angleDifference(a

, b

)/D
if rectangle(p
x
, p
y
,p

, l, w) return true
return false

2-x) Collide(a
x
,a
y
)
return disk3D(a
x
,a
y
,0,0,R) or Sphere(a
x
,a
y
,R,R)

3) One of the worst implementations, take:
W(x,y) if (a
x
-x)
2
+(a
y
-y)
2
R
2
, return true, else return false

3-i) Collide(
1
,
2
)
let b be the location of the fixed base
c = [b
x
b
y
]+l
1
.[cos(
1
) sin(
1
)] //end of link 1
d = [c
x
c
y
]+l
2
.[cos(
1
+
2
) sin(
1
+
2
)] //end of link 2
return line(b,c) or line(c,d)

3-ii) Collide(b
x
, b
y
) return disk(b
x
,b
y
,r)

3-iii) Collide (b
x
,b
y
,c
x
,c
y
)
=tan
-1
((c
y
-b
y
) / (c
x
-b
x
)) //TIP: Use atan2
for i=0 to distance(b
x
,b
y
,c
x
,c
y
) in steps //lets iterate i over the line, and at each step we'll place a disk
p=[b
x
,b
y
]

+ i.[cos() sin()] //position of disk on its way from b to c
if disk(p
x
, p
y
,r) return true
return false

Better solutions:

3-i) Use definition:
line(b
x
,b
y
,c
x
,c
y
)
Solve for :
P = ( .[b
x
b
y
]+(1-). [c
x
c
y
] ) //any general point in the line from b to c
(a
x
-p
x
)
2
+(a
y
-p
y
)
2
= R
2
//find points where the line b to c intersects the circle
to get solutions
i
if 0
i
1 return true, else return false

3-ii) Collide(b
x
, b
y
) // two disks collide if the distance between the centers is less than the sum of their radii
if ((a
x
-b
x
)
2
+(a
y
-b
y
)
2
)
1/2
R+r return true, else return false

3-iii)
Solve for :
P = (.[b
x
b
y
]+(1-). [c
x
c
y
] ) //any general point in the line from b to c
((a
x
-p
x
)
2
+(a
y
-p
y
)
2
)
1/2
= R+r //find points where the two circles touch each other
to get solutions
i
if 0
i
1 return true, else return false