New functions added in Matlab and Mathematica
DistanceTo and TestIf
This commit is contained in:
HuanWeng
parent
3d882136bf
commit
2a3f75e5ad
|
|
@ -0,0 +1,30 @@
|
|||
%*** CHAPTER 3: RIGID-BODY MOTIONS ***
|
||||
|
||||
function d = DistanceToSE3(T)
|
||||
% Takes mat: A 4x4 matrix.
|
||||
% Returns d, a quantity describing the distance of T from the SE(3)
|
||||
% manifold.
|
||||
% Compute the determinant of R, the top 3x3 submatrix of T. If
|
||||
% det(R) <= 0, return a large number. If det(R) > 0, replace the top 3x3
|
||||
% submatrix of T with R' * R, and set the first three entries of the
|
||||
% fourth column of T to zero. Then return norm(T - I).
|
||||
% Example Inputs:
|
||||
%{
|
||||
clear; clc;
|
||||
T = [1.0, 0.0, 0.0, 1.2;
|
||||
0.0, 0.1, -0.95, 1.5;
|
||||
0.0, 1.0, 0.1, -0.9;
|
||||
0.0, 0.0, 0.1, 0.98];
|
||||
d = DistanceToSE3(T)
|
||||
%}
|
||||
% Output:
|
||||
% d =
|
||||
% 0.0884
|
||||
|
||||
[R, p] = TransToRp(T);
|
||||
if det(R) > 0
|
||||
d = norm([R' * R, [0; 0; 0]; T(4, :)] - eye(4), 'fro');
|
||||
else
|
||||
d = 1e+9;
|
||||
end
|
||||
end
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
%*** CHAPTER 3: RIGID-BODY MOTIONS ***
|
||||
|
||||
function d = DistanceToSO3(R)
|
||||
% Takes mat: A 3x3 matrix.
|
||||
% Returns d, a quantity describing the distance of R from the SO(3)
|
||||
% manifold.
|
||||
% Computes the distance from R to the SO(3) manifold using the following
|
||||
% method:
|
||||
% If det(R) <= 0, return a large number.
|
||||
% If det(R) > 0, return norm(R' * R - I).
|
||||
% Example Inputs:
|
||||
%{
|
||||
clear; clc;
|
||||
R = [1.0, 0.0, 0.0;
|
||||
0.0, 0.1, -0.95;
|
||||
0.0, 1.0, 0.1];
|
||||
d = DistanceToSO3(R)
|
||||
%}
|
||||
% Output:
|
||||
% d =
|
||||
% 0.0884
|
||||
|
||||
if det(R) > 0
|
||||
d = norm(R' * R - eye(3), 'fro');
|
||||
else
|
||||
d = 1e+9;
|
||||
end
|
||||
end
|
||||
|
|
@ -0,0 +1,20 @@
|
|||
%*** CHAPTER 3: RIGID-BODY MOTIONS ***
|
||||
|
||||
function judge = TestIfSE3(T)
|
||||
% Takes mat: A 4x4 matrix T.
|
||||
% Check if T is close to or on the manifold SE(3).
|
||||
% Example Inputs:
|
||||
%{
|
||||
clear; clc;
|
||||
T = [1.0, 0.0, 0.0, 1.2;
|
||||
0.0, 0.1, -0.95, 1.5;
|
||||
0.0, 1.0, 0.1, -0.9;
|
||||
0.0, 0.0, 0.1, 0.98];
|
||||
judge = TestIfSE3(T)
|
||||
%}
|
||||
% Output:
|
||||
% dudge =
|
||||
% 0
|
||||
|
||||
judge = NearZero(DistanceToSE3(T));
|
||||
end
|
||||
|
|
@ -0,0 +1,19 @@
|
|||
%*** CHAPTER 3: RIGID-BODY MOTIONS ***
|
||||
|
||||
function judge = TestIfSO3(R)
|
||||
% Takes mat: A 3x3 matrix R.
|
||||
% Check if R is close to or on the manifold SO(3).
|
||||
% Example Inputs:
|
||||
%{
|
||||
clear; clc;
|
||||
R = [1.0, 0.0, 0.0;
|
||||
0.0, 0.1, -0.95;
|
||||
0.0, 1.0, 0.1];
|
||||
judge = TestIfSO3(R)
|
||||
%}
|
||||
% Output:
|
||||
% dudge =
|
||||
% 0
|
||||
|
||||
judge = NearZero(DistanceToSO3(R));
|
||||
end
|
||||
|
|
@ -10,10 +10,10 @@
|
|||
NotebookFileLineBreakTest
|
||||
NotebookFileLineBreakTest
|
||||
NotebookDataPosition[ 158, 7]
|
||||
NotebookDataLength[ 583129, 11307]
|
||||
NotebookOptionsPosition[ 577538, 11225]
|
||||
NotebookOutlinePosition[ 577917, 11241]
|
||||
CellTagsIndexPosition[ 577874, 11238]
|
||||
NotebookDataLength[ 594458, 11567]
|
||||
NotebookOptionsPosition[ 588870, 11485]
|
||||
NotebookOutlinePosition[ 589247, 11501]
|
||||
CellTagsIndexPosition[ 589204, 11498]
|
||||
WindowFrame->Normal*)
|
||||
|
||||
(* Beginning of Notebook Content *)
|
||||
|
|
@ -140,7 +140,8 @@ Cell[BoxData[
|
|||
RowBox[{
|
||||
RowBox[{"Abs", "[", "s", "]"}], "<",
|
||||
SuperscriptBox["10",
|
||||
RowBox[{"-", "6"}]]}]}], "\[IndentingNewLine]", "\n"}]}]], "Input",
|
||||
RowBox[{"-", "6"}]]}]}], "\[IndentingNewLine]",
|
||||
"\[IndentingNewLine]"}]}]], "Input",
|
||||
CellChangeTimes->{{3.6755319110417843`*^9, 3.675531927048277*^9}, {
|
||||
3.675531958735077*^9, 3.675532032779791*^9}, {3.6755321540719852`*^9,
|
||||
3.675532155394449*^9}, 3.67559226134205*^9, {3.675596340559066*^9,
|
||||
|
|
@ -168,8 +169,9 @@ Cell[BoxData[
|
|||
3.7391144917104807`*^9}, {3.739114551615704*^9, 3.7391145602236476`*^9}, {
|
||||
3.739115013147765*^9, 3.7391151873754435`*^9}, 3.73911531566462*^9, {
|
||||
3.7391154731340127`*^9, 3.739115545682316*^9}, {3.7391155895767775`*^9,
|
||||
3.7391156703697853`*^9}, {3.739115838530749*^9,
|
||||
3.739115863763939*^9}},ExpressionUUID->"384776d6-859b-4642-a67d-\
|
||||
3.7391156703697853`*^9}, {3.739115838530749*^9, 3.739115863763939*^9}, {
|
||||
3.7395195552681*^9,
|
||||
3.739519584492524*^9}},ExpressionUUID->"384776d6-859b-4642-a67d-\
|
||||
e130b5288bb1"]
|
||||
}, Closed]],
|
||||
|
||||
|
|
@ -1220,8 +1222,8 @@ Cell[BoxData[
|
|||
3.675596680132864*^9, 3.6755966912919407`*^9}, {3.678096115211194*^9,
|
||||
3.6780961185043497`*^9}, {3.690393628363669*^9, 3.6903936548561845`*^9},
|
||||
3.6908550611099367`*^9, {3.6924695183676157`*^9, 3.6924695606541176`*^9}, {
|
||||
3.6931534607233896`*^9,
|
||||
3.6931534623769927`*^9}},ExpressionUUID->"231c024d-75c6-45bd-a348-\
|
||||
3.6931534607233896`*^9, 3.6931534623769927`*^9}, {3.739516946823781*^9,
|
||||
3.7395169590948253`*^9}},ExpressionUUID->"231c024d-75c6-45bd-a348-\
|
||||
b4411df7ad47"],
|
||||
|
||||
Cell[BoxData[
|
||||
|
|
@ -3075,7 +3077,247 @@ Cell[BoxData[
|
|||
RowBox[{
|
||||
RowBox[{"1", ";;", "3"}], ",", "4"}], "]"}], "]"}], "}"}],
|
||||
"\[Transpose]"}]}], "]"}]}], "\[IndentingNewLine]",
|
||||
"\[IndentingNewLine]"}]}]], "Input",
|
||||
"\[IndentingNewLine]", "\[IndentingNewLine]",
|
||||
RowBox[{"(*",
|
||||
RowBox[{
|
||||
RowBox[{
|
||||
RowBox[{"DistanceToSO3", "[", "R_", "]"}], ":", "\[IndentingNewLine]",
|
||||
" ",
|
||||
RowBox[{"Takes", " ", "a", " ", "3", "x3", " ",
|
||||
RowBox[{"matrix", ".", "\[IndentingNewLine]", "Returns"}], " ", "a",
|
||||
" ", "quantity", " ", "describing", " ", "the", " ", "distance", " ",
|
||||
"of", " ", "R", " ", "from", " ", "the", " ", "SO",
|
||||
RowBox[{"(", "3", ")"}], " ",
|
||||
RowBox[{"manifold", ".", "\[IndentingNewLine]", "Computes"}], " ",
|
||||
"the", " ", "distance", " ", "from", " ", "R", " ", "to", " ", "the",
|
||||
" ", "SO",
|
||||
RowBox[{"(", "3", ")"}], " ", "manifold", " ", "using", " ", "the",
|
||||
" ", "following", " ", "method"}], ":", " ",
|
||||
RowBox[{
|
||||
RowBox[{"If", " ", "det",
|
||||
RowBox[{"(", "R", ")"}]}], " ", "\[LessEqual]", " ", "0"}]}], ",",
|
||||
" ",
|
||||
RowBox[{
|
||||
RowBox[{"return", " ", "a", " ", "large", " ",
|
||||
RowBox[{"number", ".", " ", "If"}], " ", "det",
|
||||
RowBox[{"(", "R", ")"}]}], " ", ">", " ", "0"}], ",",
|
||||
RowBox[{
|
||||
RowBox[{"return", " ", "norm",
|
||||
RowBox[{
|
||||
RowBox[{"(",
|
||||
RowBox[{
|
||||
RowBox[{
|
||||
RowBox[{"R", "'"}], ".", "R"}], " ", "-", " ", "I"}], ")"}],
|
||||
"."}]}], "\[IndentingNewLine]", ";", "\[IndentingNewLine]",
|
||||
RowBox[{
|
||||
RowBox[{
|
||||
"Example", ":", "\[IndentingNewLine]", "Input", ":", " ", "d"}], " ",
|
||||
"=", " ",
|
||||
RowBox[{
|
||||
RowBox[{"DistanceToSO3", "[",
|
||||
RowBox[{"{",
|
||||
RowBox[{
|
||||
RowBox[{"{",
|
||||
RowBox[{"1.0", ",", "0.0", ",", "0.0"}], "}"}], ",",
|
||||
RowBox[{"{",
|
||||
RowBox[{"0.0", ",", "0.1", ",",
|
||||
RowBox[{"-", "0.95"}]}], "}"}], ",",
|
||||
RowBox[{"{",
|
||||
RowBox[{"0.0", ",", "1.0", ",", "0.1"}], "}"}]}], "}"}], "]"}],
|
||||
"\[IndentingNewLine]",
|
||||
RowBox[{"Output", ":", " ",
|
||||
TagBox[
|
||||
TagBox["0.08835298523536149",
|
||||
Function[BoxForm`e$,
|
||||
MatrixForm[BoxForm`e$]]],
|
||||
Function[BoxForm`e$,
|
||||
MatrixForm[BoxForm`e$]]]}]}]}]}]}], "\[IndentingNewLine]", "*)"}],
|
||||
"\[IndentingNewLine]", "\[IndentingNewLine]",
|
||||
RowBox[{
|
||||
RowBox[{"DistanceToSO3", "[", "R_", "]"}], ":=",
|
||||
RowBox[{"If", "[",
|
||||
RowBox[{
|
||||
RowBox[{
|
||||
RowBox[{"Det", "[", "R", "]"}], ">", "0"}], ",",
|
||||
RowBox[{"Return", "[",
|
||||
RowBox[{"Norm", "[",
|
||||
RowBox[{
|
||||
RowBox[{
|
||||
RowBox[{
|
||||
RowBox[{"R", "\[Transpose]"}], ".", "R"}], "-",
|
||||
RowBox[{"IdentityMatrix", "[", "3", "]"}]}], ",",
|
||||
"\"\<Frobenius\>\""}], "]"}], "]"}], ",",
|
||||
RowBox[{"Return", "[",
|
||||
SuperscriptBox["10", "9"], "]"}]}], "]"}]}], "\[IndentingNewLine]",
|
||||
"\[IndentingNewLine]", "\[IndentingNewLine]",
|
||||
RowBox[{"(*",
|
||||
RowBox[{
|
||||
RowBox[{
|
||||
RowBox[{"DistanceToSE3", "[", "T_", "]"}], ":", "\[IndentingNewLine]",
|
||||
" ",
|
||||
RowBox[{"Takes", " ", "a", " ", "4", "x4", " ",
|
||||
RowBox[{"matrix", ".", "\[IndentingNewLine]", "Returns"}], " ", "a",
|
||||
" ", "quantity", " ", "describing", " ", "the", " ", "distance", " ",
|
||||
"of", " ", "T", " ", "from", " ", "the", " ", "SE",
|
||||
RowBox[{"(", "3", ")"}], " ",
|
||||
RowBox[{"manifold", ".", "\[IndentingNewLine]", "Compute"}], " ",
|
||||
"the", " ", "determinant", " ", "of", " ", "R"}]}], ",", " ",
|
||||
RowBox[{
|
||||
RowBox[{
|
||||
"the", " ", "top", " ", "3", "x3", " ", "submatrix", " ", "of", " ",
|
||||
RowBox[{"T", ".", " ", "If"}], " ", "det",
|
||||
RowBox[{"(", "R", ")"}]}], " ", "\[LessEqual]", " ", "0"}], ",", " ",
|
||||
RowBox[{
|
||||
RowBox[{"return", " ", "a", " ", "large", " ",
|
||||
RowBox[{"number", ".", " ", "If"}], " ", "det",
|
||||
RowBox[{"(", "R", ")"}]}], " ", ">", " ", "0"}], ",", " ",
|
||||
RowBox[{
|
||||
"replace", " ", "the", " ", "top", " ", "3", "x3", " ", "submatrix", " ",
|
||||
"of", " ", "T", " ", "with", " ",
|
||||
RowBox[{
|
||||
RowBox[{"R", "'"}], ".", "R"}]}], ",", " ",
|
||||
RowBox[{
|
||||
RowBox[{
|
||||
"and", " ", "set", " ", "the", " ", "first", " ", "three", " ",
|
||||
"entries", " ", "of", " ", "the", " ", "fourth", " ", "column", " ",
|
||||
"of", " ", "T", " ", "to", " ",
|
||||
RowBox[{"zero", ".", " ", "Then"}], " ", "return", " ", "norm",
|
||||
RowBox[{
|
||||
RowBox[{"(",
|
||||
RowBox[{"T", " ", "-", " ", "I"}], ")"}], "."}]}],
|
||||
"\[IndentingNewLine]", ";", "\[IndentingNewLine]",
|
||||
RowBox[{
|
||||
RowBox[{
|
||||
"Example", ":", "\[IndentingNewLine]", "Input", ":", " ", "d"}], " ",
|
||||
"=", " ",
|
||||
RowBox[{
|
||||
RowBox[{"DistanceToSE3", "[",
|
||||
RowBox[{"{",
|
||||
RowBox[{
|
||||
RowBox[{"{",
|
||||
RowBox[{"1.0", ",", "0.0", ",", "0.0", ",", "1.2"}], "}"}], ",",
|
||||
RowBox[{"{",
|
||||
RowBox[{"0.0", ",", "0.1", ",",
|
||||
RowBox[{"-", "0.95"}], ",", "1.5"}], "}"}], ",",
|
||||
RowBox[{"{",
|
||||
RowBox[{"0.0", ",", "1.0", ",", "0.1", ",",
|
||||
RowBox[{"-", "0.9"}]}], "}"}], ",",
|
||||
RowBox[{"{",
|
||||
RowBox[{"0.0", ",", "0.0", ",", "0.1", ",", "0.98"}], "}"}]}],
|
||||
"}"}], "]"}], "\[IndentingNewLine]",
|
||||
RowBox[{"Output", ":", " ", "0.13493053768513638"}]}]}]}]}],
|
||||
"\[IndentingNewLine]", "*)"}], "\[IndentingNewLine]",
|
||||
"\[IndentingNewLine]",
|
||||
RowBox[{
|
||||
RowBox[{"DistanceToSE3", "[", "T_", "]"}], ":=",
|
||||
RowBox[{"Module", "[",
|
||||
RowBox[{
|
||||
RowBox[{"{",
|
||||
RowBox[{"R", ",", "p"}], "}"}], ",",
|
||||
RowBox[{
|
||||
RowBox[{
|
||||
RowBox[{"{",
|
||||
RowBox[{"R", ",", "p"}], "}"}], "=",
|
||||
RowBox[{"TransToRp", "[", "T", "]"}]}], ";",
|
||||
RowBox[{"If", "[",
|
||||
RowBox[{
|
||||
RowBox[{
|
||||
RowBox[{"Det", "[", "R", "]"}], ">", "0"}], ",",
|
||||
RowBox[{"Return", "[",
|
||||
RowBox[{"Norm", "[",
|
||||
RowBox[{
|
||||
RowBox[{
|
||||
RowBox[{"Join", "[",
|
||||
RowBox[{
|
||||
RowBox[{"Join", "[",
|
||||
RowBox[{
|
||||
RowBox[{
|
||||
RowBox[{"R", "\[Transpose]"}], ".", "R"}], ",",
|
||||
RowBox[{
|
||||
RowBox[{"{",
|
||||
RowBox[{"{",
|
||||
RowBox[{"0", ",", "0", ",", "0"}], "}"}], "}"}],
|
||||
"\[Transpose]"}], ",", "2"}], "]"}], ",",
|
||||
RowBox[{"{",
|
||||
RowBox[{"T", "[",
|
||||
RowBox[{"[",
|
||||
RowBox[{"4", ",", ";;"}], "]"}], "]"}], "}"}]}], "]"}], "-",
|
||||
RowBox[{"IdentityMatrix", "[", "4", "]"}]}], ",",
|
||||
"\"\<Frobenius\>\""}], "]"}], "]"}], ",",
|
||||
RowBox[{"Return", "[",
|
||||
SuperscriptBox["10", "9"], "]"}]}], "]"}]}]}], "]"}]}],
|
||||
"\[IndentingNewLine]", "\[IndentingNewLine]", "\[IndentingNewLine]",
|
||||
RowBox[{"(*",
|
||||
RowBox[{
|
||||
RowBox[{
|
||||
RowBox[{"TestIfSO3", "[", "R_", "]"}], ":", "\[IndentingNewLine]", " ",
|
||||
RowBox[{"Takes", " ", "a", " ", "3", "x3", " ",
|
||||
RowBox[{"matrix", ".", "\[IndentingNewLine]", "Check"}], " ", "if",
|
||||
" ", "R", " ", "is", " ", "close", " ", "to", " ", "or", " ", "on",
|
||||
" ", "the", " ", "manifold", " ", "SO",
|
||||
RowBox[{
|
||||
RowBox[{"(", "3", ")"}], "."}]}]}], "\[IndentingNewLine]", ";",
|
||||
"\[IndentingNewLine]",
|
||||
RowBox[{
|
||||
RowBox[{
|
||||
"Example", ":", "\[IndentingNewLine]", "Input", ":", " ", "judge"}],
|
||||
" ", "=", " ",
|
||||
RowBox[{
|
||||
RowBox[{"TestIfSO3", "[",
|
||||
RowBox[{"{",
|
||||
RowBox[{
|
||||
RowBox[{"{",
|
||||
RowBox[{"1.0", ",", "0.0", ",", "0.0"}], "}"}], ",",
|
||||
RowBox[{"{",
|
||||
RowBox[{"0.0", ",", "0.1", ",",
|
||||
RowBox[{"-", "0.95"}]}], "}"}], ",",
|
||||
RowBox[{"{",
|
||||
RowBox[{"0.0", ",", "1.0", ",", "0.1"}], "}"}]}], "}"}], "]"}],
|
||||
"\[IndentingNewLine]",
|
||||
RowBox[{"Output", ":", " ", "False"}]}]}]}], "\[IndentingNewLine]",
|
||||
"*)"}], "\[IndentingNewLine]", "\[IndentingNewLine]",
|
||||
RowBox[{
|
||||
RowBox[{"TestIfSO3", "[", "R_", "]"}], ":=",
|
||||
RowBox[{"NearZero", "[",
|
||||
RowBox[{"DistanceToSO3", "[", "R", "]"}], "]"}]}], "\[IndentingNewLine]",
|
||||
"\[IndentingNewLine]", "\[IndentingNewLine]",
|
||||
RowBox[{"(*",
|
||||
RowBox[{
|
||||
RowBox[{
|
||||
RowBox[{"TestIfSE3", "[", "T_", "]"}], ":", "\[IndentingNewLine]", " ",
|
||||
RowBox[{"Takes", " ", "a", " ", "4", "X4", " ",
|
||||
RowBox[{"matrix", ".", "\[IndentingNewLine]", "Check"}], " ", "if",
|
||||
" ", "T", " ", "is", " ", "close", " ", "to", " ", "or", " ", "on",
|
||||
" ", "the", " ", "manifold", " ", "SE",
|
||||
RowBox[{
|
||||
RowBox[{"(", "3", ")"}], "."}]}]}], "\[IndentingNewLine]", ";",
|
||||
"\[IndentingNewLine]",
|
||||
RowBox[{
|
||||
RowBox[{
|
||||
"Example", ":", "\[IndentingNewLine]", "Input", ":", " ", "judge"}],
|
||||
" ", "=", " ",
|
||||
RowBox[{
|
||||
RowBox[{"TestIfSE3", "[",
|
||||
RowBox[{"{",
|
||||
RowBox[{
|
||||
RowBox[{"{",
|
||||
RowBox[{"1.0", ",", "0.0", ",", "0.0", ",", "1.2"}], "}"}], ",",
|
||||
RowBox[{"{",
|
||||
RowBox[{"0.0", ",", "0.1", ",",
|
||||
RowBox[{"-", "0.95"}], ",", "1.5"}], "}"}], ",",
|
||||
RowBox[{"{",
|
||||
RowBox[{"0.0", ",", "1.0", ",", "0.1", ",",
|
||||
RowBox[{"-", "0.9"}]}], "}"}], ",",
|
||||
RowBox[{"{",
|
||||
RowBox[{"0.0", ",", "0.0", ",", "0.1", ",", "0.98"}], "}"}]}],
|
||||
"}"}], "]"}], "\[IndentingNewLine]",
|
||||
RowBox[{"Output", ":", " ", "False"}]}]}]}], "\[IndentingNewLine]",
|
||||
"*)"}], "\[IndentingNewLine]", "\[IndentingNewLine]",
|
||||
RowBox[{
|
||||
RowBox[{"TestIfSE3", "[", "T_", "]"}], ":=",
|
||||
RowBox[{"NearZero", "[",
|
||||
RowBox[{"DistanceToSE3", "[", "T", "]"}], "]"}]}], "\[IndentingNewLine]",
|
||||
"\[IndentingNewLine]"}]}]], "Input",
|
||||
CellChangeTimes->{{3.654107438701495*^9, 3.6541075770931664`*^9}, {
|
||||
3.6541076103152647`*^9, 3.654107726674172*^9}, {3.654107800327899*^9,
|
||||
3.654107805926005*^9}, {3.654107842157054*^9, 3.654107956534466*^9}, {
|
||||
|
|
@ -3219,10 +3461,28 @@ Cell[BoxData[
|
|||
3.739122564870072*^9, 3.7391225688833127`*^9}, {3.739123129003651*^9,
|
||||
3.7391231526767683`*^9}, {3.739123758931058*^9, 3.7391237867909555`*^9}, {
|
||||
3.7391239502498274`*^9, 3.7391240409630785`*^9}, {3.739124739958088*^9,
|
||||
3.7391247482039185`*^9}, {3.7391264152358837`*^9,
|
||||
3.739126432068795*^9}},ExpressionUUID->"30cfbd3e-57bb-4e33-99e1-\
|
||||
3.7391247482039185`*^9}, {3.7391264152358837`*^9, 3.739126432068795*^9}, {
|
||||
3.7395146084854665`*^9, 3.739514699211993*^9}, 3.7395147442384024`*^9, {
|
||||
3.7395147760686655`*^9, 3.739514867035931*^9}, {3.739514901868358*^9,
|
||||
3.739514947185063*^9}, {3.739515012201562*^9, 3.7395151874695163`*^9}, {
|
||||
3.739515920019891*^9, 3.739516111097062*^9}, {3.7395161736187277`*^9,
|
||||
3.7395163487390146`*^9}, {3.7395164508221674`*^9, 3.7395165602717085`*^9},
|
||||
3.7395165957193995`*^9, {3.7395166458480487`*^9, 3.739516670257716*^9}, {
|
||||
3.7395169216744976`*^9, 3.739516922105482*^9}, {3.7395170302279854`*^9,
|
||||
3.7395170305826592`*^9}, {3.7395171091216917`*^9, 3.739517113000117*^9}, {
|
||||
3.739517202741659*^9, 3.739517214371004*^9}, {3.7395174820271325`*^9,
|
||||
3.7395174847737727`*^9}, {3.7395175192625136`*^9, 3.739517573167054*^9}, {
|
||||
3.7395176093518763`*^9, 3.7395176442463894`*^9}, {3.7395177075281024`*^9,
|
||||
3.739517768961048*^9}, {3.739517815270845*^9, 3.7395178339003735`*^9}, {
|
||||
3.7395180608532534`*^9, 3.739518069774658*^9}, 3.739518110440982*^9, {
|
||||
3.7395181723659773`*^9, 3.7395182259215794`*^9}, {3.739518355020355*^9,
|
||||
3.7395183583134203`*^9}, 3.7395183909234786`*^9, {3.739518695106036*^9,
|
||||
3.7395188785015717`*^9}, {3.7395189108690205`*^9,
|
||||
3.7395189759945483`*^9}, {3.739519028200582*^9, 3.7395190632221794`*^9}, {
|
||||
3.7395194255986805`*^9, 3.7395195339550247`*^9}, {3.7395196265136285`*^9,
|
||||
3.7395197395141363`*^9}},ExpressionUUID->"30cfbd3e-57bb-4e33-99e1-\
|
||||
c83444da6e3e"]
|
||||
}, Open ]],
|
||||
}, Closed]],
|
||||
|
||||
Cell[CellGroupData[{
|
||||
|
||||
|
|
@ -7106,9 +7366,9 @@ Cell[BoxData[
|
|||
RowBox[{"The", " ", "resulting", " ", "joint", " ",
|
||||
RowBox[{"accelerations", ".", "\[IndentingNewLine]", "This"}], " ",
|
||||
"function", " ", "computes", " ", "ddthetalist", " ", "by", " ",
|
||||
RowBox[{"solving", ":", "\[IndentingNewLine]",
|
||||
RowBox[{"Mlist",
|
||||
RowBox[{"(", "thetalist", ")"}], "ddthetalist"}]}]}]}]}], "=",
|
||||
"solving"}], ":", "\[IndentingNewLine]",
|
||||
RowBox[{"Mlist",
|
||||
RowBox[{"(", "thetalist", ")"}], "ddthetalist"}]}]}], "=",
|
||||
RowBox[{"taulist", "-",
|
||||
RowBox[{"c",
|
||||
RowBox[{"(",
|
||||
|
|
@ -8576,7 +8836,7 @@ Cell[BoxData[
|
|||
3.6931539707350855`*^9}, {3.7306524103938313`*^9,
|
||||
3.7306524111785803`*^9}},ExpressionUUID->"c001454f-a821-4609-9c44-\
|
||||
fc644598fd8e"]
|
||||
}, Open ]],
|
||||
}, Closed]],
|
||||
|
||||
Cell[CellGroupData[{
|
||||
|
||||
|
|
@ -11224,7 +11484,7 @@ Cell[BoxData[
|
|||
}, Closed]]
|
||||
},
|
||||
WindowSize->{1904, 997},
|
||||
WindowMargins->{{-94, Automatic}, {Automatic, 89}},
|
||||
WindowMargins->{{3, Automatic}, {Automatic, 44}},
|
||||
Magnification:>1.1 Inherited,
|
||||
FrontEndVersion->"11.2 for Microsoft Windows (64-bit) (September 10, 2017)",
|
||||
StyleDefinitions->"Default.nb"
|
||||
|
|
@ -11242,72 +11502,72 @@ CellTagsIndex->{}
|
|||
Notebook[{
|
||||
Cell[CellGroupData[{
|
||||
Cell[580, 22, 863, 20, 149, "Title",ExpressionUUID->"3096a67c-add9-43da-83af-32b7a7a080ef"],
|
||||
Cell[1446, 44, 1977, 38, 940, "Title",ExpressionUUID->"b6567309-8434-4708-a2bb-37b3f2220626"],
|
||||
Cell[3426, 84, 953, 14, 63, "Input",ExpressionUUID->"051fe601-9dc3-4e99-996b-456fe895c2ee"]
|
||||
Cell[1446, 44, 1977, 38, 1137, "Title",ExpressionUUID->"b6567309-8434-4708-a2bb-37b3f2220626"],
|
||||
Cell[3426, 84, 953, 14, 75, "Input",ExpressionUUID->"051fe601-9dc3-4e99-996b-456fe895c2ee"]
|
||||
}, Closed]],
|
||||
Cell[CellGroupData[{
|
||||
Cell[4416, 103, 428, 8, 48, "Title",ExpressionUUID->"67b2b117-d9a7-4270-b942-f78127e03058"],
|
||||
Cell[4847, 113, 3254, 59, 308, "Input",ExpressionUUID->"384776d6-859b-4642-a67d-e130b5288bb1"]
|
||||
Cell[4847, 113, 3323, 61, 280, "Input",ExpressionUUID->"384776d6-859b-4642-a67d-e130b5288bb1"]
|
||||
}, Closed]],
|
||||
Cell[CellGroupData[{
|
||||
Cell[8138, 177, 408, 8, 48, "Title",ExpressionUUID->"634fe91f-27b4-4e3f-a2ab-11a4d248e7fd"],
|
||||
Cell[8549, 187, 2290, 53, 275, "Input",ExpressionUUID->"f6c93c67-d27e-41b6-bdde-6f032fb810cd"],
|
||||
Cell[10842, 242, 3262, 86, 275, "Input",ExpressionUUID->"a30bf937-6481-47ee-bb9e-be8b63cba8e1"],
|
||||
Cell[14107, 330, 2717, 70, 275, "Input",ExpressionUUID->"c7a81d49-344f-4e62-a20b-5d1c78342f7c"],
|
||||
Cell[16827, 402, 2903, 64, 297, "Input",ExpressionUUID->"a1df8796-c264-4486-93a4-7068200bf83e"],
|
||||
Cell[19733, 468, 5841, 124, 382, "Input",ExpressionUUID->"9e5fc67d-eedb-4d6c-92ce-f7e1bae8e4b6"],
|
||||
Cell[25577, 594, 10433, 233, 429, "Input",ExpressionUUID->"305f1770-a098-48b5-a75d-b4d53949ef1a"],
|
||||
Cell[36013, 829, 11703, 195, 340, "Input",ExpressionUUID->"4ddea111-4d1b-4fab-bb2e-2bf18b5511c8"],
|
||||
Cell[47719, 1026, 11790, 198, 319, "Input",ExpressionUUID->"231c024d-75c6-45bd-a348-b4411df7ad47"],
|
||||
Cell[59512, 1226, 12296, 212, 384, "Input",ExpressionUUID->"6b00b4cf-8c90-4a79-8772-4318b9207225"],
|
||||
Cell[71811, 1440, 11679, 199, 340, "Input",ExpressionUUID->"2d25915a-29df-4bc1-87fb-88bf40588e9c"],
|
||||
Cell[83493, 1641, 11520, 193, 275, "Input",ExpressionUUID->"8a71ceb0-ae72-491a-a009-dd65a1a09814"],
|
||||
Cell[95016, 1836, 11997, 204, 388, "Input",ExpressionUUID->"883d455b-d605-47ff-8abc-4fc42cf1f167"],
|
||||
Cell[107016, 2042, 11644, 192, 319, "Input",ExpressionUUID->"a4b8d789-2193-4eb4-89e8-f131c17e84af"],
|
||||
Cell[118663, 2236, 12248, 208, 319, "Input",ExpressionUUID->"ec6248e6-c8a6-47c2-b31f-5eddd1d42a35"],
|
||||
Cell[130914, 2446, 16513, 297, 406, "Input",ExpressionUUID->"cb2b4b25-a328-4451-b1a7-53449ff8db62"],
|
||||
Cell[147430, 2745, 23859, 478, 1128, "Input",ExpressionUUID->"30cfbd3e-57bb-4e33-99e1-c83444da6e3e"]
|
||||
}, Open ]],
|
||||
Cell[CellGroupData[{
|
||||
Cell[171326, 3228, 358, 7, 77, "Title",ExpressionUUID->"2c8ea6eb-fa7d-47e8-92f0-27b5858ae3f0"],
|
||||
Cell[171687, 3237, 7193, 167, 567, "Input",ExpressionUUID->"42e3cd6c-7142-4593-924a-642228db11d2"],
|
||||
Cell[178883, 3406, 6633, 157, 567, "Input",ExpressionUUID->"3e604bd8-121f-4823-845b-8af9015f2e0b"]
|
||||
Cell[8207, 179, 408, 8, 48, "Title",ExpressionUUID->"634fe91f-27b4-4e3f-a2ab-11a4d248e7fd"],
|
||||
Cell[8618, 189, 2290, 53, 275, "Input",ExpressionUUID->"f6c93c67-d27e-41b6-bdde-6f032fb810cd"],
|
||||
Cell[10911, 244, 3262, 86, 275, "Input",ExpressionUUID->"a30bf937-6481-47ee-bb9e-be8b63cba8e1"],
|
||||
Cell[14176, 332, 2717, 70, 275, "Input",ExpressionUUID->"c7a81d49-344f-4e62-a20b-5d1c78342f7c"],
|
||||
Cell[16896, 404, 2903, 64, 297, "Input",ExpressionUUID->"a1df8796-c264-4486-93a4-7068200bf83e"],
|
||||
Cell[19802, 470, 5841, 124, 382, "Input",ExpressionUUID->"9e5fc67d-eedb-4d6c-92ce-f7e1bae8e4b6"],
|
||||
Cell[25646, 596, 10433, 233, 429, "Input",ExpressionUUID->"305f1770-a098-48b5-a75d-b4d53949ef1a"],
|
||||
Cell[36082, 831, 11703, 195, 340, "Input",ExpressionUUID->"4ddea111-4d1b-4fab-bb2e-2bf18b5511c8"],
|
||||
Cell[47788, 1028, 11838, 198, 319, "Input",ExpressionUUID->"231c024d-75c6-45bd-a348-b4411df7ad47"],
|
||||
Cell[59629, 1228, 12296, 212, 384, "Input",ExpressionUUID->"6b00b4cf-8c90-4a79-8772-4318b9207225"],
|
||||
Cell[71928, 1442, 11679, 199, 340, "Input",ExpressionUUID->"2d25915a-29df-4bc1-87fb-88bf40588e9c"],
|
||||
Cell[83610, 1643, 11520, 193, 275, "Input",ExpressionUUID->"8a71ceb0-ae72-491a-a009-dd65a1a09814"],
|
||||
Cell[95133, 1838, 11997, 204, 388, "Input",ExpressionUUID->"883d455b-d605-47ff-8abc-4fc42cf1f167"],
|
||||
Cell[107133, 2044, 11644, 192, 319, "Input",ExpressionUUID->"a4b8d789-2193-4eb4-89e8-f131c17e84af"],
|
||||
Cell[118780, 2238, 12248, 208, 319, "Input",ExpressionUUID->"ec6248e6-c8a6-47c2-b31f-5eddd1d42a35"],
|
||||
Cell[131031, 2448, 16513, 297, 406, "Input",ExpressionUUID->"cb2b4b25-a328-4451-b1a7-53449ff8db62"],
|
||||
Cell[147547, 2747, 35088, 736, 2258, "Input",ExpressionUUID->"30cfbd3e-57bb-4e33-99e1-c83444da6e3e"]
|
||||
}, Closed]],
|
||||
Cell[CellGroupData[{
|
||||
Cell[185553, 3568, 424, 8, 48, "Title",ExpressionUUID->"000ed3dd-e640-4394-bfdd-112e054fe39e"],
|
||||
Cell[185980, 3578, 16119, 289, 547, "Input",ExpressionUUID->"240e2dfc-5e90-4b05-a228-e4214db5aa73"],
|
||||
Cell[202102, 3869, 14782, 263, 572, "Input",ExpressionUUID->"537c0d96-762a-44d8-9b3b-ae7cd65b0ad8"]
|
||||
Cell[182672, 3488, 358, 7, 48, "Title",ExpressionUUID->"2c8ea6eb-fa7d-47e8-92f0-27b5858ae3f0"],
|
||||
Cell[183033, 3497, 7193, 167, 516, "Input",ExpressionUUID->"42e3cd6c-7142-4593-924a-642228db11d2"],
|
||||
Cell[190229, 3666, 6633, 157, 516, "Input",ExpressionUUID->"3e604bd8-121f-4823-845b-8af9015f2e0b"]
|
||||
}, Closed]],
|
||||
Cell[CellGroupData[{
|
||||
Cell[216921, 4137, 462, 9, 48, "Title",ExpressionUUID->"9b65060d-851c-4e01-9b60-18f3c42fd7f6"],
|
||||
Cell[217386, 4148, 23816, 427, 834, "Input",ExpressionUUID->"faf42c91-d222-47ba-a1cf-c70438e66ed9"],
|
||||
Cell[241205, 4577, 24229, 435, 883, "Input",ExpressionUUID->"cd8261cd-edb7-439b-80ea-07f7f0f661f2"]
|
||||
Cell[196899, 3828, 424, 8, 48, "Title",ExpressionUUID->"000ed3dd-e640-4394-bfdd-112e054fe39e"],
|
||||
Cell[197326, 3838, 16119, 289, 498, "Input",ExpressionUUID->"240e2dfc-5e90-4b05-a228-e4214db5aa73"],
|
||||
Cell[213448, 4129, 14782, 263, 520, "Input",ExpressionUUID->"537c0d96-762a-44d8-9b3b-ae7cd65b0ad8"]
|
||||
}, Closed]],
|
||||
Cell[CellGroupData[{
|
||||
Cell[265471, 5017, 510, 9, 48, "Title",ExpressionUUID->"81e5be89-ffaf-473a-8cbb-55c594f704e9"],
|
||||
Cell[265984, 5028, 10924, 193, 476, "Input",ExpressionUUID->"eebfa9bb-9b2c-44e6-9326-456220a27d55"],
|
||||
Cell[276911, 5223, 31869, 641, 1155, "Input",ExpressionUUID->"d13bd5d8-2632-4dc0-b0b1-630524e24a38"],
|
||||
Cell[308783, 5866, 17547, 351, 807, "Input",ExpressionUUID->"92528685-883e-4c0c-bac2-bf106210e42b"],
|
||||
Cell[326333, 6219, 15720, 310, 783, "Input",ExpressionUUID->"f3f61900-aa02-4fd1-9e7e-e8d496ab43f7"],
|
||||
Cell[342056, 6531, 15016, 298, 738, "Input",ExpressionUUID->"a7efa093-4b6e-496e-9dcb-6d6052ee3445"],
|
||||
Cell[357075, 6831, 8797, 216, 738, "Input",ExpressionUUID->"050d5074-a82f-46cd-af46-1b1b25e99315"],
|
||||
Cell[365875, 7049, 16719, 337, 955, "Input",ExpressionUUID->"d6589c3c-8957-41cb-b657-352eb72af4ed"],
|
||||
Cell[382597, 7388, 10341, 186, 520, "Input",ExpressionUUID->"6009f6be-153c-4a23-9e78-8febfb6f128a"],
|
||||
Cell[392941, 7576, 22510, 468, 1294, "Input",ExpressionUUID->"8c71c553-e0e9-4531-9417-f427517894a6"],
|
||||
Cell[415454, 8046, 25638, 531, 1439, "Input",ExpressionUUID->"c001454f-a821-4609-9c44-fc644598fd8e"]
|
||||
}, Open ]],
|
||||
Cell[CellGroupData[{
|
||||
Cell[441129, 8582, 513, 9, 84, "Title",ExpressionUUID->"75d6083f-b722-498a-b9fa-576327a19efc"],
|
||||
Cell[441645, 8593, 15363, 235, 751, "Input",ExpressionUUID->"c0075b7e-c880-4998-8fed-6d7994aad149"],
|
||||
Cell[457011, 8830, 2877, 69, 751, "Input",ExpressionUUID->"b650297d-45e5-44b0-83f7-ea7a549e74e8"],
|
||||
Cell[459891, 8901, 17437, 315, 1716, "Input",ExpressionUUID->"01b172e6-150d-47a2-93d8-0c226f061852"],
|
||||
Cell[477331, 9218, 19665, 378, 2270, "Input",ExpressionUUID->"8eab3c39-2044-482c-9bc9-f59721338bd9"],
|
||||
Cell[496999, 9598, 21722, 421, 2362, "Input",ExpressionUUID->"e7f7c189-2377-47aa-8f37-8943a95caef1"]
|
||||
Cell[228267, 4397, 462, 9, 48, "Title",ExpressionUUID->"9b65060d-851c-4e01-9b60-18f3c42fd7f6"],
|
||||
Cell[228732, 4408, 23816, 427, 759, "Input",ExpressionUUID->"faf42c91-d222-47ba-a1cf-c70438e66ed9"],
|
||||
Cell[252551, 4837, 24229, 435, 803, "Input",ExpressionUUID->"cd8261cd-edb7-439b-80ea-07f7f0f661f2"]
|
||||
}, Closed]],
|
||||
Cell[CellGroupData[{
|
||||
Cell[518758, 10024, 504, 9, 52, "Title",ExpressionUUID->"10d566a6-4644-4e65-8900-e5ebea810780"],
|
||||
Cell[519265, 10035, 25323, 468, 1447, "Input",ExpressionUUID->"beb700cb-b16d-4bc9-a775-f33065f66984"],
|
||||
Cell[544591, 10505, 32931, 717, 3138, "Input",ExpressionUUID->"78a6cb02-1e69-4815-87fd-0b13c84611e1"]
|
||||
Cell[276817, 5277, 510, 9, 48, "Title",ExpressionUUID->"81e5be89-ffaf-473a-8cbb-55c594f704e9"],
|
||||
Cell[277330, 5288, 10924, 193, 476, "Input",ExpressionUUID->"eebfa9bb-9b2c-44e6-9326-456220a27d55"],
|
||||
Cell[288257, 5483, 31869, 641, 1155, "Input",ExpressionUUID->"d13bd5d8-2632-4dc0-b0b1-630524e24a38"],
|
||||
Cell[320129, 6126, 17547, 351, 734, "Input",ExpressionUUID->"92528685-883e-4c0c-bac2-bf106210e42b"],
|
||||
Cell[337679, 6479, 15720, 310, 712, "Input",ExpressionUUID->"f3f61900-aa02-4fd1-9e7e-e8d496ab43f7"],
|
||||
Cell[353402, 6791, 15016, 298, 671, "Input",ExpressionUUID->"a7efa093-4b6e-496e-9dcb-6d6052ee3445"],
|
||||
Cell[368421, 7091, 8797, 216, 671, "Input",ExpressionUUID->"050d5074-a82f-46cd-af46-1b1b25e99315"],
|
||||
Cell[377221, 7309, 16705, 337, 869, "Input",ExpressionUUID->"d6589c3c-8957-41cb-b657-352eb72af4ed"],
|
||||
Cell[393929, 7648, 10341, 186, 473, "Input",ExpressionUUID->"6009f6be-153c-4a23-9e78-8febfb6f128a"],
|
||||
Cell[404273, 7836, 22510, 468, 1177, "Input",ExpressionUUID->"8c71c553-e0e9-4531-9417-f427517894a6"],
|
||||
Cell[426786, 8306, 25638, 531, 1309, "Input",ExpressionUUID->"c001454f-a821-4609-9c44-fc644598fd8e"]
|
||||
}, Closed]],
|
||||
Cell[CellGroupData[{
|
||||
Cell[452461, 8842, 513, 9, 48, "Title",ExpressionUUID->"75d6083f-b722-498a-b9fa-576327a19efc"],
|
||||
Cell[452977, 8853, 15363, 235, 385, "Input",ExpressionUUID->"c0075b7e-c880-4998-8fed-6d7994aad149"],
|
||||
Cell[468343, 9090, 2877, 69, 385, "Input",ExpressionUUID->"b650297d-45e5-44b0-83f7-ea7a549e74e8"],
|
||||
Cell[471223, 9161, 17437, 315, 762, "Input",ExpressionUUID->"01b172e6-150d-47a2-93d8-0c226f061852"],
|
||||
Cell[488663, 9478, 19665, 378, 956, "Input",ExpressionUUID->"8eab3c39-2044-482c-9bc9-f59721338bd9"],
|
||||
Cell[508331, 9858, 21722, 421, 1002, "Input",ExpressionUUID->"e7f7c189-2377-47aa-8f37-8943a95caef1"]
|
||||
}, Closed]],
|
||||
Cell[CellGroupData[{
|
||||
Cell[530090, 10284, 504, 9, 48, "Title",ExpressionUUID->"10d566a6-4644-4e65-8900-e5ebea810780"],
|
||||
Cell[530597, 10295, 25323, 468, 1067, "Input",ExpressionUUID->"beb700cb-b16d-4bc9-a775-f33065f66984"],
|
||||
Cell[555923, 10765, 32931, 717, 2101, "Input",ExpressionUUID->"78a6cb02-1e69-4815-87fd-0b13c84611e1"]
|
||||
}, Closed]]
|
||||
}
|
||||
]
|
||||
|
|
|
|||
Loading…
Reference in New Issue