Learn Julia (13): 선형 대수 및 회전
25614 단어 julialinearalgebraroboticsrotation
공간의 3개 축과 그에 따른 회전에 대한 프리젠테이션:
이제
roll
, pitch
및 yaw
행렬을 코딩해 보겠습니다.using LinearAlgebra
## roll: about x-axis
function getRotx(theta::Float64, angleInDeg = false)
if (angleInDeg)
theta = theta*pi/180.0
end
return [1 0 0; 0 cos(theta) -sin(theta); 0 sin(theta) cos(theta)]
end
## pitch: about y-axis
function getRoty(theta::Float64, angleInDeg = false)
if (angleInDeg)
theta = theta*pi/180.0
end
return [cos(theta) 0 sin(theta); 0 1 0;-sin(theta) 0 cos(theta)]
end
## yaw: about z-axis
function getRotz(theta::Float64, angleInDeg = false)
if (angleInDeg)
theta = theta*pi/180.0
end
return [cos(theta) -sin(theta) 0; sin(theta) cos(theta) 0;0 0 1]
end
수학에서 몇 가지 중요한 그룹 정의:
차원 3의 직교 그룹의 이름은
SO(3)
로 로봇 공학에서 매우 많이 사용됩니다. 3D 공간의 모든 회전 행렬은 이 그룹SO(3)
에 속합니다.## check if a matrix belongs to group SO(3)
function isInSO3(R)
res = ( size(R) == (3, 3) )
if res
res = (transpose(R) * R == I)
end
res
end
이제 회전, Vec3, 쿼터니언 및 관련 기능을 정의합니다.
# Identity matrix for 3D space
I33 = [1 0 0; 0 1 0; 0 0 1]
## definition of a 3D vector Datatype
## for representing a vector in space
struct Vec3
v1::Float64
v2::Float64
v3::Float64
end
# converting a Vec3 to a 3x1 array
function Vec3toArray(v::Vec3)
return [v.v1, v.v2, v.v3]
end
## A rotation in 3d space consists of an axis and an angle about the axis
struct Rot3
unitAxis::Vec3
thetaInRad::Float64
end
# Quaternion datatype
mutable struct Quaternion
q0::Float64
q1::Float64
q2::Float64
q3::Float64
end
# converting a 3d rotation to a Quaternion respresentation
function getQuaternion(R::Rot3)
q = Quaternion(1.0, 0.0, 0.0, 0.0)
axis = Vec3toArray(R.unitAxis)
if norm(axis) - 1.0 > 1e-10
println("warning: not unit axis")
axis = axis./norm(axis)
end
sin_theta = sin(0.5*R.thetaInRad)
q.q0 = cos(0.5*R.thetaInRad)
q.q1 = axis[1]*sin_theta
q.q2 = axis[2]*sin_theta
q.q3 = axis[3]*sin_theta
return q
end
## convert a 3d vector to its skew-matrix form which belongs to group so(3)
function getSkew3(v::Vec3)
return [ 0 -v.v3 v.v2;
v.v3 0 -v.v1;
-v.v2 v.v1 0]
end
## converting a 3d rotation to a matrix respresentation
function getRotMatrix(r::Rot3)
q = getQuaternion(r)
# omega = [q.q1, q.q1, q.q1]
if r.thetaInRad == 0.0 #TODO: also include 2n*pi
omega = Vec3(0.0, 0.0, 0.0)
else
alpha = sin( 0.5* r.thetaInRad )
omega = Vec3(q.q1/alpha, q.q2/alpha, q.q3/alpha)
end
sk = getSkew3(omega)
println("omega: $omega")
println("skew: ", sk)
return I33 .+ sin( r.thetaInRad )* sk .+ (1 - cos( r.thetaInRad ))* sk* sk
end
이제 위에서 정의한 함수를 테스트합니다.
## rotate about z-axis for 0.3*pi
r1 = Rot3( Vec3(0.0,0.0,1.0), 0.3*pi)
Mrot1 = getRotMatrix(r1)
## rotate about x-axis for 0.2pi
r2 = Rot3( Vec3(1.0,0.0,0.0), 0.2*pi)
Mrot2 = getRotMatrix(r2)
## rotate about y-axis for 0.15pi
r3 = Rot3( Vec3(0.0,1.0,0.0), 0.15*pi)
Mrot3 = getRotMatrix(r3)
println("Mrot1: $Mrot1 ")
println("det(Mrot1): ", det(Mrot1))
println("trace(Mrot1): ", tr(Mrot1))
println("inv(Mrot1): ", inv(Mrot1))
## check if the results are correct
println(Mrot1 .- getRotz(0.3*pi))
println(Mrot2 .- getRotx(0.2*pi))
println(Mrot3 .- getRoty(0.15*pi))
이제 약간의 시각화 테스트를 해보겠습니다. 아래 코드에서
points
는 z축에 대해 45도 회전해야 합니다.points = [0 0 0.5; 1 0 0.5 ]
x = points[:,1]
y = points[:,2]
z = points[:,3]
r4 = Rot3( Vec3(0.0,0.0,1.0), 0.25*pi) # 45 degree about z-axis
Mrot4 = getRotMatrix(r4)
points_new = transpose(Mrot4* transpose(points))
xn = points_new[:,1]
yn = points_new[:,2]
zn = points_new[:,3]
plotting = true
if plotting
using Plots
plotlyjs()
display(plot(x, y, z, xlabel = "x", ylabel = "y", zlabel ="z", w = 10))
display(plot!(xn, yn, zn, w = 10))
display(plot!(zeros(11), zeros(11), 0:0.1:1, w = 3))
display(plot!(zeros(11), 0:0.1:1, zeros(11), w = 3) )
display(plot!(0:0.1:1, zeros(11), zeros(11), w = 3) )
end
표시된 3D 차트에서 두 개의 스냅샷을 잘라냈는데, 점
[1 0 0.5]
이 z축에 대해 45도 회전된 것을 볼 수 있습니다.Reference
이 문제에 관하여(Learn Julia (13): 선형 대수 및 회전), 우리는 이곳에서 더 많은 자료를 발견하고 링크를 클릭하여 보았다 https://dev.to/jemaloqiu/learn-julia-12-linear-algebra-and-rotations-4n83텍스트를 자유롭게 공유하거나 복사할 수 있습니다.하지만 이 문서의 URL은 참조 URL로 남겨 두십시오.
우수한 개발자 콘텐츠 발견에 전념 (Collection and Share based on the CC Protocol.)