diff --git a/.gitignore b/.gitignore index d6e1269..f05cfa2 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ + # OS generated files # ###################### .DS_Store diff --git a/ProgramFiles/Utilities/LieGroups.m b/ProgramFiles/Utilities/LieGroups.m index d782eb3..7422dc0 100644 --- a/ProgramFiles/Utilities/LieGroups.m +++ b/ProgramFiles/Utilities/LieGroups.m @@ -20,7 +20,7 @@ % matrix mapping function fn = mat_fn(group) if group == LieGroups.SE2 - fn = @vec_to_mat_SE2; + fn = @vec_to_mat_SE2_lie; elseif group == LieGroups.SO3 fn = @vec_to_mat_SO3; end @@ -28,7 +28,7 @@ % vector mapping function fn = vec_fn(group) if group == LieGroups.SE2 - fn = @mat_to_vec_SE2; + fn = @mat_to_vec_SE2_lie; elseif group == LieGroups.SO3 fn = @mat_to_vec_SO3; end diff --git a/ProgramFiles/Utilities/kinematics/mat_to_vec_SE2_lie.m b/ProgramFiles/Utilities/kinematics/mat_to_vec_SE2_lie.m new file mode 100644 index 0000000..1851e21 --- /dev/null +++ b/ProgramFiles/Utilities/kinematics/mat_to_vec_SE2_lie.m @@ -0,0 +1,6 @@ +function vector = mat_to_vec_SE2_lie(matrix) +%Convert SE(2) matrices to a set of row vectors + +vector = [matrix(1,3);matrix(2,3);matrix(2,1)]; + +end \ No newline at end of file diff --git a/ProgramFiles/Utilities/kinematics/vec_to_mat_SE2_lie.m b/ProgramFiles/Utilities/kinematics/vec_to_mat_SE2_lie.m new file mode 100644 index 0000000..e80f89d --- /dev/null +++ b/ProgramFiles/Utilities/kinematics/vec_to_mat_SE2_lie.m @@ -0,0 +1,9 @@ +function matrix = vec_to_mat_SE2_lie(vector) + +theta = vector(3); +x = vector(1); +y = vector(2); + +matrix = [0,-theta,x;theta,0,y;0,0,0]; + +end diff --git a/ProgramFiles/sys_calcsystem_fcns/calc_ccf.m b/ProgramFiles/sys_calcsystem_fcns/calc_ccf.m index 5c3104a..957bb68 100644 --- a/ProgramFiles/sys_calcsystem_fcns/calc_ccf.m +++ b/ProgramFiles/sys_calcsystem_fcns/calc_ccf.m @@ -36,8 +36,10 @@ end % iter. thru J (all rows, col < row): entry = 1; - for r = 1:size(A,2) - for c = 1:r +% for r = 1:size(A,2) +% for c = 1:r + for c = 1:size(A,2) + for r = c:size(A,2) % combine like bases (respecting wedge product rules) % if doing a different choice of basis, change: % which entry (r,c) pairs are saved in @@ -47,7 +49,7 @@ continue; end % two partials for this basis - dA{i,entry} = -partials{r,c} + partials{c,r}; + dA{i,entry} = partials{r,c} - partials{c,r}; entry = entry + 1; end @@ -66,8 +68,10 @@ lb_pts{i} = zeros(size(dA)); % iterate thru. combinations of bases entry = 1; - for aj = 1:size(A,2) - for ak = 1:aj +% for aj = 1:size(A,2) +% for ak = 1:aj + for ak = 1:size(A,2) + for aj = ak:size(A,2) % skip zero wedge products if aj == ak continue; @@ -86,6 +90,7 @@ %% combine for DA DA = cell(size(dA)); for i = 1:numel(dA) - DA{i} = -(dA{i} + lb{i}); %sign choice for consistency + DA{i} = dA{i} - lb{i}; %sign choice for consistency end + end \ No newline at end of file