Download presentation
Presentation is loading. Please wait.
1
Solving position analysis for a triangle constraint
Law of Sines 𝑎 sin 𝛼 = 𝑏 sin 𝛽 = c sin 𝛾 c a b 𝛽 𝛾 𝛼 Law of cosines 𝑎 2 = 𝑏 2 + 𝑐 2 −2𝑏𝑐 cos 𝛼
2
𝒃 3 𝒃 2 𝒃 1 𝒆 1 𝒆 2 𝒆 3 𝒂 1 𝒂 2 𝒂 3
3
Adding Servo Motors 𝒃 3 𝒃 2 𝒃 1 𝒆 1 𝒆 2 𝒆 3 𝒂 1 𝒂 2 𝒂 3
4
Define Motor Angles 𝑞 2 𝒃 3 𝒃 2 𝒃 1 𝒆 1 𝒆 2 𝒆 3 𝒂 1 𝒂 2 𝒂 3 𝑞 1 𝑞 3
5
How Do We Find the Motor Angles
Problem: given points a1, a2, a3 and b1 b2 b3 and the link lengths calculate q1 q2 q3 𝑞 2 𝒃 3 𝒃 2 𝒃 1 𝒆 1 𝒆 2 𝒆 3 𝒂 1 𝒂 2 𝒂 3 𝑞 1 𝑞 3
6
How Do We Find the Motor Angles
c a b 𝛽 𝛾 𝛼 𝑞 2 𝒃 2 𝒆 2 Law of cosines 𝑎 2 = 𝑏 2 + 𝑐 2 −2𝑏𝑐 cos 𝛼 𝒂 2 𝒂 3 𝒆 1 𝑞 1 𝒂 1 𝒃 3 𝒃 1 𝑞 3 𝒆 3
7
How do we find the length 𝒃 𝟏 𝒆 1 ?
Law of cosines 𝑎 2 = 𝑏 2 + 𝑐 2 −2𝑏𝑐 cos 𝛼 c a b 𝛽 𝛾 𝛼 cos 𝛼 = 𝑏 2 + 𝑐 2 − 𝑎 2 2𝑏𝑐 𝛼 = 𝑎𝑟𝑐𝑐𝑜𝑠 𝑏 2 + 𝑐 2 − 𝑎 2 2𝑏𝑐 𝒆 1 𝒂 𝟏 𝒆 1 =𝑎 𝒂 1 𝒂 𝟏 𝒃 1 =𝑏 𝛼 1 𝑞 1 𝒃 𝟏 𝒂 1 =𝑐 𝒃 1 How do we find the length 𝒃 𝟏 𝒆 1 ?
8
Length of a segment defined by two points
𝒃 𝟏 𝒆 1 =𝑐= 𝑏 𝑥 − 𝑎 𝑥 𝑏 𝑦 − 𝑎 𝑦 2 𝒂 1 = 𝑎 𝑥 , 𝑎 𝑦 𝑎 𝑦 𝒃 1 = 𝑏 𝑥 , 𝑏 𝑦 𝑏 𝑦 𝑎 𝑥 𝑏 𝑥 𝒆 1 𝒂 𝟏 𝒆 1 =𝑎 𝒂 1 = 𝑎 𝑥 , 𝑎 𝑦 𝒂 𝟏 𝒃 1 =𝑏 𝛼 1 𝑞 1 𝒃 𝟏 𝒂 1 =𝑐 𝒃 1 𝒃 1 = 𝑏 𝑥 , 𝑏 𝑦
9
Law of cosines 𝑎 2 = 𝑏 2 + 𝑐 2 −2𝑏𝑐 cos 𝛼 c a b 𝛽 𝛾 𝛼 cos 𝛼 = 𝑏 2 + 𝑐 2 − 𝑎 2 2𝑏𝑐 𝒃 1 𝒂 1 𝑞 1 𝛼 1 𝒃 𝟏 𝒂 1 𝒂 𝟏 𝒆 1 = 𝐿 2 𝒃 𝟏 𝒆 1 = 𝐿 1 𝛼 = 𝑎𝑟𝑐𝑐𝑜𝑠 𝐿 𝑎 1 𝑏 − 𝐿 𝑎 1 𝑏 1 𝐿 1 How do we find the point 𝒂 1 so we can calculate the length 𝑐?
10
How Do We Find the Motor Angles
Problem: given point p, angle 𝜃 and the size of the triangle find points 𝒂 1 , 𝒂 2 , 𝒂 3 𝒃 3 𝒃 2 𝒃 1 𝒆 1 𝒆 2 𝒆 3 𝒂 1 𝒂 2 𝒂 3 𝑞 1 𝑞 2 𝑞 3 𝒑 𝜃
11
Finding points 𝑎 1 , 𝑎 2 and 𝑎 3 𝑟 𝑝 𝑎 1𝑥 = 𝑝 x +rcos 𝜃− 𝜋 6
𝑎 1𝑦 = 𝑝 y +r 𝑠𝑖𝑛 𝜃− 𝜋 6 𝑎 2𝑥 = 𝑝 x +rcos 𝜃+ 𝜋 2 𝑎 2𝑦 = 𝑝 y +r sin 𝜃+ 𝜋 2 𝑎 3𝑥 = 𝑝 x +rcos 𝜃+ 7𝜋 6 𝑎 3𝑦 = 𝑝 y +r sin 𝜃+ 7𝜋 6 𝑞 2 𝒃 3 𝒃 2 𝒃 1 𝒆 1 𝒆 2 𝒆 3 𝒂 1 𝒂 2 𝒂 3 𝑟 𝑝 𝒑 𝜃 𝑞 1 𝑞 3
12
Finding points 𝑏 1 , 𝑏 2 and 𝑏 3 𝑟 𝑏 𝑏 1𝑥 = r b cos − 𝜋 6 = 𝑟 𝑏 3 2
𝑏 2𝑦 = r b sin 𝜋 2 =𝑟 𝑏 3𝑥 = r b cos 7𝜋 6 = 𝑟 𝑏 𝑏 3𝑦 = r b 𝑠𝑖𝑛 7𝜋 6 = − 𝑟 𝑏 2 𝑞 2 𝒃 3 𝒃 2 𝒃 1 𝒆 1 𝒆 2 𝒆 3 𝒂 1 𝒂 2 𝒂 3 𝑟 𝑏 𝒑 𝑞 1 𝑞 3
13
Need slide for explaining atan2 visually
14
Finding the motor angles
𝜓 2 =𝑎𝑡𝑎𝑛2(𝑎2𝑦−𝑏2𝑦, 𝑎2𝑥−𝑏2𝑥) We will make sure that atan2 returns angles between 0 and 360 degrees 𝜓 2 𝑞 2 𝒃 3 𝒃 2 𝒃 1 𝒆 1 𝒆 2 𝒆 3 𝒂 1 𝒂 2 𝒂 3 𝑞 2 = 𝜓 2 − 𝛼 2 − 2𝜋 3 𝛼 2 𝜓 1 =𝑎𝑡𝑎𝑛2(𝑎1𝑦−𝑏1𝑦, 𝑎1𝑥−𝑏1𝑥) 𝒑 𝑞 1 = 𝜓 1 − 𝛼 1 𝜃 𝜓 1 𝜓 3 𝛼 1 𝑞 1 𝛼 3 𝑞 3 𝑞 3 = 𝜓 3 − 𝛼 3 − 4𝜋 3 𝜓 3 =𝑎𝑡𝑎𝑛2(𝑎3𝑦−𝑏3𝑦, 𝑎3𝑥−𝑏3𝑥)
15
Numerical values for our robots
𝑟_𝑝=0.04 𝑚 𝑟_𝑏=0.1 𝑚 L1 = L2 = b1x = b1y = b2x = 0 b2y = b3x = b3y = theta = px = 0 py = 0 a1x = a1y = a2x = 0 a2y = a3x = a3y =
Similar presentations
