You are on page 1of 6

# >> syms d L t x y B real

## >> oTf = transl ([d, 0, 0])* rotz(t) * transl ([L,0,0])

oTf =
[ cos(t), -sin(t), 0, d + L*cos(t)]
[ sin(t), cos(t), 0,
L*sin(t)]
[
0,
0, 1,
0]
[
0,
0, 0,
1]
>> fTp = transl ([x,y,0]) * rotz (B)
fTp =
[ cos(B), -sin(B), 0, x]
[ sin(B), cos(B), 0, y]
[
0,
0, 1, 0]
[
0,
0, 0, 1]
>> oTp = oTf*fTp
oTp =
[
)
[
)
[

## cos(B)*cos(t) - sin(B)*sin(t), - cos(B)*sin(t) - sin(B)*cos(t), 0, d + L*cos(t

+ x*cos(t) - y*sin(t)]
cos(B)*sin(t) + sin(B)*cos(t), cos(B)*cos(t) - sin(B)*sin(t), 0,
L*sin(t
+ y*cos(t) + x*sin(t)]
0,
0, 1,
0]
[
0,
0, 0,
1]
>> simple(oTp)
simplify:
[ cos(B + t), -sin(B + t), 0, d + L*cos(t) + x*cos(t) - y*sin(t)]
[ sin(B + t), cos(B + t), 0,
L*sin(t) + y*cos(t) + x*sin(t)]
[
0,
0, 1,
0]
[
0,
0, 0,
1]
radsimp:
[
)
[
)
[

## cos(B)*cos(t) - sin(B)*sin(t), - cos(B)*sin(t) - sin(B)*cos(t), 0, d + L*cos(t

+ x*cos(t) - y*sin(t)]
cos(B)*sin(t) + sin(B)*cos(t), cos(B)*cos(t) - sin(B)*sin(t), 0,
L*sin(t
+ y*cos(t) + x*sin(t)]
0,
0, 1,
0]
[
0,
0, 0,
1]
simplify(100):
[ cos(B + t), -sin(B + t), 0, d + L*cos(t) + x*cos(t) - y*sin(t)]
[ sin(B + t), cos(B + t), 0,
L*sin(t) + y*cos(t) + x*sin(t)]
[
0,
0, 1,
0]
[
0,
0, 0,
1]

combine(sincos):
[ cos(B + t), -sin(B + t), 0, d + L*cos(t) + x*cos(t) - y*sin(t)]
[ sin(B + t), cos(B + t), 0,
L*sin(t) + y*cos(t) + x*sin(t)]
[
0,
0, 1,
0]
[
0,
0, 0,
1]
combine(sinhcosh):
[
)
[
)
[

## cos(B)*cos(t) - sin(B)*sin(t), - cos(B)*sin(t) - sin(B)*cos(t), 0, d + L*cos(t

+ x*cos(t) - y*sin(t)]
cos(B)*sin(t) + sin(B)*cos(t), cos(B)*cos(t) - sin(B)*sin(t), 0,
L*sin(t
+ y*cos(t) + x*sin(t)]
0,
0, 1,
0]
[
0,
0, 0,
1]
combine(ln):
[
)
[
)
[

## cos(B)*cos(t) - sin(B)*sin(t), - cos(B)*sin(t) - sin(B)*cos(t), 0, d + L*cos(t

+ x*cos(t) - y*sin(t)]
cos(B)*sin(t) + sin(B)*cos(t), cos(B)*cos(t) - sin(B)*sin(t), 0,
L*sin(t
+ y*cos(t) + x*sin(t)]
0,
0, 1,
0]
[
0,
0, 0,
1]
factor:
[
)
[
)
[

## cos(B)*cos(t) - sin(B)*sin(t), - cos(B)*sin(t) - sin(B)*cos(t), 0, d + L*cos(t

+ x*cos(t) - y*sin(t)]
cos(B)*sin(t) + sin(B)*cos(t), cos(B)*cos(t) - sin(B)*sin(t), 0,
L*sin(t
+ y*cos(t) + x*sin(t)]
0,
0, 1,
0]
[
0,
0, 0,
1]
expand:
[
)
[
)
[

## cos(B)*cos(t) - sin(B)*sin(t), - cos(B)*sin(t) - sin(B)*cos(t), 0, d + L*cos(t

+ x*cos(t) - y*sin(t)]
cos(B)*sin(t) + sin(B)*cos(t), cos(B)*cos(t) - sin(B)*sin(t), 0,
L*sin(t
+ y*cos(t) + x*sin(t)]
0,
0, 1,
0]
[
0,
0, 0,
1]
combine:

[
)
[
)
[

## cos(B)*cos(t) - sin(B)*sin(t), - cos(B)*sin(t) - sin(B)*cos(t), 0, d + L*cos(t

+ x*cos(t) - y*sin(t)]
cos(B)*sin(t) + sin(B)*cos(t), cos(B)*cos(t) - sin(B)*sin(t), 0,
L*sin(t
+ y*cos(t) + x*sin(t)]
0,
0, 1,
0]
[
0,
0, 0,
1]
rewrite(exp):
[ ((1/exp(B*i))/2 + exp(B*i)/2)*((1/exp(t*i))/2 + exp(t*i)/2) - (((1/exp(B*i))*i
)/2 - (exp(B*i)*i)/2)*(((1/exp(t*i))*i)/2 - (exp(t*i)*i)/2), - ((1/exp(B*i))/2 +
exp(B*i)/2)*(((1/exp(t*i))*i)/2 - (exp(t*i)*i)/2) - (((1/exp(B*i))*i)/2 - (exp(
B*i)*i)/2)*((1/exp(t*i))/2 + exp(t*i)/2), 0,
d + L*((1/exp(t*i))/2 + exp(t*i
)/2) + x*((1/exp(t*i))/2 + exp(t*i)/2) - y*(((1/exp(t*i))*i)/2 - (exp(t*i)*i)/2)
]
[ ((1/exp(B*i))/2 + exp(B*i)/2)*(((1/exp(t*i))*i)/2 - (exp(t*i)*i)/2) + (((1/exp
(B*i))*i)/2 - (exp(B*i)*i)/2)*((1/exp(t*i))/2 + exp(t*i)/2), ((1/exp(B*i))/2 +
exp(B*i)/2)*((1/exp(t*i))/2 + exp(t*i)/2) - (((1/exp(B*i))*i)/2 - (exp(B*i)*i)/
2)*(((1/exp(t*i))*i)/2 - (exp(t*i)*i)/2), 0, L*(((1/exp(t*i))*i)/2 - (exp(t*i)*i
)/2) + x*(((1/exp(t*i))*i)/2 - (exp(t*i)*i)/2) + y*((1/exp(t*i))/2 + exp(t*i)/2)
]
[
0,
0, 1,
0
]
[
0,
0, 0,
1
]
rewrite(sincos):
[
)
[
)
[

## cos(B)*cos(t) - sin(B)*sin(t), - cos(B)*sin(t) - sin(B)*cos(t), 0, d + L*cos(t

+ x*cos(t) - y*sin(t)]
cos(B)*sin(t) + sin(B)*cos(t), cos(B)*cos(t) - sin(B)*sin(t), 0,
L*sin(t
+ y*cos(t) + x*sin(t)]
0,
0, 1,
0]
[
0,
0, 0,
1]
rewrite(sinhcosh):
[

## cosh(-B*i)*cosh(-t*i) + sinh(-B*i)*sinh(-t*i), - cosh(-B*i)*sinh(-t*i)*i sinh(-B*i)*cosh(-t*i)*i, 0, d + x*cosh(-t*i) - y*sinh(-t*i)*i + L*cosh(-t*i)]

[ cosh(-B*i)*sinh(-t*i)*i + sinh(-B*i)*cosh(-t*i)*i,
cosh(-B*i)*cosh(-t*i)
+ sinh(-B*i)*sinh(-t*i), 0, y*cosh(-t*i) + x*sinh(-t*i)*i + L*sinh(-t*i)*i]
[
0,
0, 1,
0]
[
0,
0, 0,
1]

rewrite(tan):
[
((tan(t/2)^2 - 1)*(tan(B/2)^2 - 1))/((tan(t/2)^2 + 1)*(tan(B/2)^2 + 1)) (4*tan(B/2)*tan(t/2))/((tan(t/2)^2 + 1)*(tan(B/2)^2 + 1)), (2*tan(B/2)*(tan(t/2)
^2 - 1))/((tan(t/2)^2 + 1)*(tan(B/2)^2 + 1)) + (2*tan(t/2)*(tan(B/2)^2 - 1))/((t
an(t/2)^2 + 1)*(tan(B/2)^2 + 1)), 0, d - (L*(tan(t/2)^2 - 1))/(tan(t/2)^2 + 1) (x*(tan(t/2)^2 - 1))/(tan(t/2)^2 + 1) - (2*y*tan(t/2))/(tan(t/2)^2 + 1)]
[ - (2*tan(B/2)*(tan(t/2)^2 - 1))/((tan(t/2)^2 + 1)*(tan(B/2)^2 + 1)) - (2*tan(t
/2)*(tan(B/2)^2 - 1))/((tan(t/2)^2 + 1)*(tan(B/2)^2 + 1)), ((tan(t/2)^2 - 1)*(
tan(B/2)^2 - 1))/((tan(t/2)^2 + 1)*(tan(B/2)^2 + 1)) - (4*tan(B/2)*tan(t/2))/((t
an(t/2)^2 + 1)*(tan(B/2)^2 + 1)), 0,
(2*L*tan(t/2))/(tan(t/2)^2 + 1) (y*(tan(t/2)^2 - 1))/(tan(t/2)^2 + 1) + (2*x*tan(t/2))/(tan(t/2)^2 + 1)]
[
0,
0, 1,
0]
[
0,
0, 0,
1]
mwcos2sin:
[ (2*sin(B/2)^2 - 1)*(2*sin(t/2)^2 - 1) - sin(B)*sin(t), sin(B)*(2*sin(t/2)^2
- 1) + sin(t)*(2*sin(B/2)^2 - 1), 0, d - L*(2*sin(t/2)^2 - 1) - x*(2*sin(t/2)^2
- 1) - y*sin(t)]
[ - sin(B)*(2*sin(t/2)^2 - 1) - sin(t)*(2*sin(B/2)^2 - 1), (2*sin(B/2)^2 - 1)*(2
*sin(t/2)^2 - 1) - sin(B)*sin(t), 0,
L*sin(t) - y*(2*sin(t/2)^2
- 1) + x*sin(t)]
[
0,
0, 1,
0]
[
0,
0, 0,
1]
collect(x):
[
)
[
)
[

## cos(B)*cos(t) - sin(B)*sin(t), - cos(B)*sin(t) - sin(B)*cos(t), 0, d + L*cos(t

+ x*cos(t) - y*sin(t)]
cos(B)*sin(t) + sin(B)*cos(t), cos(B)*cos(t) - sin(B)*sin(t), 0,
L*sin(t
+ y*cos(t) + x*sin(t)]
0,
0, 1,
0]
[
0,
0, 0,
1]
ans =
[ cos(B + t), -sin(B + t), 0, d + L*cos(t) + x*cos(t) - y*sin(t)]
[ sin(B + t), cos(B + t), 0,
L*sin(t) + y*cos(t) + x*sin(t)]
[
0,
0, 1,
0]
[
0,
0, 0,
1]

>> x=0
x =
0
>> y=0
y =
0
>> t=0
t =
0
>> eval (oTp)
ans =
[ cos(B), -sin(B), 0, L + d]
[ sin(B), cos(B), 0,
0]
[
0,
0, 1,
0]
[
0,
0, 0,
1]
>> L=3
L =
3
>> d=2
d =
2
>> eval (oTp)
ans =
[ cos(B), -sin(B), 0, 5]
[ sin(B), cos(B), 0, 0]
[
0,
0, 1, 0]
[
0,
0, 0, 1]
>> B=30
B =
30
>> eval (oTp)

ans =
0.1543
-0.9880
0
0

0.9880
0.1543
0
0

0
0
1.0000
0

5.0000
0
0
1.0000