screws.m

《机器人数学导论》中的matlab程序

(* Extract the angular portion of a twist [private] *)
xitow[xi_?VectorQ] :=
  Module[
    {n = xidim[xi]},

    (* Make sure that the vector had a reasonable length *)   
    If[n == 0, Return Null];

    (* Extract the angular portion of the twist *)
    Take[xi, -(n (n-1) / 2)]
  ];

(* Extract the linear portion of a twist [private] *)
xitov[xi_?VectorQ] :=
  Module[
    {n = xidim[xi]},

    (* Make sure that the vector had a reasonable length *)   
    If[n == 0, Return Null];

    (* Extract the linear portion of the twist *)
    Take[xi, n]
  ];

(* Check to see if a matrix is a twist matrix *)
(*! Not implemented !*)
TwistMatrixQ[A_] := MatrixQ[A];

(* Convert a homogeneous matrix to a twist *)
(*! This only works in dimensions 2 and 3 for now !*)
HomogeneousToTwist[A_] :=
  Module[
    {nr, nc},

    (* Check to make sure that our input makes sense *)
    If[Not[MatrixQ[A]] || ({nr, nc} = Dimensions[A]; nr != nc),
        Message[Screws::wrongDimensions, "HomogeneousToTwist"];
	Return Null;
    ];

    (* Make sure that we have a twist and not a rigid motion *)
    If[A[[nr,nc]] != 0,
        Message[Screws::notTwistMatrix, "HomogeneousToTwist"];
	Return Null;
    ];

    (* Extract the skew part and the vector part and make a vector *)
    Join[
      Flatten[extractSubMatrix[A, {1,nr-1}, {nc}]],
      SkewToAxis[ extractSubMatrix[A, {1,nr-1}, {1,nc-1}] ]
    ]
  ];

(* Convert a twist to homogeneous coordinates *)
TwistToHomogeneous[xi_?VectorQ] :=
  Module[
    {w = xitow[xi], v = xitov[xi], R, p},
    
    (* Make sure that we got a real twist *)
    If[w == Null || v == NULL, Return Null];

    (* Now put everything together into a homogeneous transformation *)
    StackCols[
      StackRows[AxisToSkew[w], zeroMatrix[1, Length[w]]],
      StackRows[v, {0}]
    ]
  ];  

(* Take the exponential of a twist *)
(*! This only works in dimension 3 for now !*)
TwistExp[xi_?MatrixQ, theta_:1]:=TwistExp[HomogeneousToTwist[xi], theta]; 
TwistExp[xi_?VectorQ, theta_:1] :=
  Module[
    {w = xitow[xi], v = xitov[xi], R, p},
    
    (* Make sure that we got a real twist *)
    If[w == Null || v == NULL, Return Null];

    (* Use the exponential formula from MLS *)
    If [(MatchQ[w,{0,0,0}] || MatchQ[w, {{0},{0},{0}}]),
      R = IdentityMatrix[3];
      p = v * theta,
     (* else *)
      R = SkewExp[w, theta];
      p = (IdentityMatrix[3] - R) . (AxisToSkew[w] . v) + w (w.v) theta;
    ];
    RPToHomogeneous[R, p]
  ];

(* Find the twist which generates a rigid motion *)
RigidTwist[g_?MatrixQ] :=
  Module[
    {R, p, axis, v, theta},

    (* Make sure the dimensions are okay *)
    (*! Missing !*)

    (* Extract the appropriate pieces of the homogeneous transformation *)
    R = RigidOrientation[g];
    p = RigidPosition[g];

    (* Now find the axis from the rotation *)    
    w = RotationAxis[R];
    theta = RotationAngle[R];

    (* Split into cases depending on whether theta is zero *)
    If[theta == 0,
      theta = Magnitude[p];
      v = p/Magnitude[p];
      w = 0;
    (* else *)
      (* Solve a linear equation to figure out what v is *)   
      v = LinearSolve[
        (IdentityMatrix[3]-Outer[Times,w,w]) Sin[theta] +
        Skew[w] (1 - Cos[theta]) + Outer[Times,w,w] theta,
      p]
    ];
    Flatten[{v, w}]
  ];

(*
 * Geometric attributes of twists and wrenches.
 *
 * For twists in R^3, find the attributes of that twist.
 *
 * Wrench attributes are defined by switching the role of linear
 * and angular portions
 *)

(* Build a twist from a screw *)
ScrewToTwist[Infinity, q_, w_] := Join[w, {0,0,0}];
ScrewToTwist[h_, q_, w_] := Join[-AxisToSkew[w] . q + h w, w]

(* Find the pitch associated with a twist in R^3 *)
TwistPitch[xi_?VectorQ] := 
  Module[
    {v, w},
    {v, w} = Partition[xi, 3];
    v . w / w.w
  ];
WrenchPitch[xi_?VectorQ] := Null;

(* Find the axis of a twist *)
TwistAxis[xi_?VectorQ] := 
  Module[
    {v, w},
    {v, w} = Partition[xi, 3];
    If[(MatchQ[w,{0,0,0}] || MatchQ[w, {{0},{0},{0}}]), 
     {0, v / Sqrt[v.v]}, {AxisToSkew[w] . v / w.w, (w / w.w)}]
  ];

WrenchAxis[xi_?VectorQ] := Null;

(* Find the magnitude of a twist *)
TwistMagnitude[xi_?VectorQ] := 
  Module[
    {v, w},
    {v, w} = Partition[xi, 3];
    If[(MatchQ[w,{0,0,0}] || MatchQ[w, {{0},{0},{0}}]), 
      Sqrt[v.v], Sqrt[w.w]]
  ];
WrenchMagnitude[xi_?VectorQ] := Null;

(* Inverse matrix calculation *)
(*! This only works in R^3 !*)
RigidInverse[g_?MatrixQ] := 
  Module[
    {R = RigidOrientation[g], p = RigidPosition[g]},
    RPToHomogeneous[Transpose[R], -Transpose[R].p]
  ];


(*
 * Adjoint calculation
 *
 * The adjoint matrix maps twist vectors to twist vectors.
 *
 *)

(* Adjoint matrix calculation *)
(*! This only works in R^3 !*)
RigidAdjoint[g_?MatrixQ] := 
  Module[
    {R = RigidOrientation[g], p = RigidPosition[g]},
    StackRows[
        StackCols[R, AxisToSkew[p] . R],
	StackCols[zeroMatrix[3], R]	
    ]
  ];


(*
 * Predicate (query) functions
 *
 * Define predicates to test for the various types of objects which
 * occur in Screw theory.
 *
 * RotationQ	rotation matrix
 * skewQ	skew symmetric [private]
 *)

(* check to see if a matrix is a rotation matrix (any dimension) *)
RotationQ[mat_] :=
  Module[
    {nr, nc, zmat},

    (* First check to make sure that this is square matrix *)
    If[Not[MatrixQ[mat]] || ({nr, nc} = Dimensions[mat]; nr != nc),
	Message[Screws::notSquare];    
        Return[False]];

    (* Check to see if R^T R = Identity *)
    zmat = Simplify[mat . Transpose[mat]] - IdentityMatrix[nr];
    Return[ And @@ Map[TrueQ[Simplify[#] == 0]&, Flatten[zmat]]]
  ];

skewQ[mat_] :=
  Module[
    {nr, nc, zmat},

    (* First check to make sure that this is square matrix *)
    If[Not[MatrixQ[mat]] || ({nr, nc} = Dimensions[mat]; nr != nc),
	Message[Screws::notSquare];    
        Return[False]];

    (* Check to see if A = -A^T *)
    zmat = mat + Transpose[mat];
    Return[ And @@ Map[TrueQ[Simplify[#] == 0]&, Flatten[zmat]]]
];

(*
 * Graphics functions
 *
 * DrawScrew		generate a graphical representation of a screw
 *
 *)

(* Define default options for screws *)
Options[DrawScrew] = {ScrewSize->2}

(* Draw a screw through the point q, in direction w, with pitch h *)
DrawScrew[q_, w_, h_:0, opts___Rule] :=
  Module[
    {x, y, z, R, axis, tip},

    (* Generate the rotation to get things to align with the tip *)
    z = w / Sqrt[w.w];
    y = NullSpace[{z}][[1]];	y = y / Sqrt[y.y];
    x = AxisToSkew[y] . z;
    R = Transpose[{x,y,z}];

    (* Generate the arrow for the tip *)
    tip := TranslateShape[
      Graphics3D[
        Cone[ScrewSize/20, ScrewSize/20, 10] /.
          {Polygon[list_] :> Polygon[Map[R . #&, list]]} ],
      q + ScrewSize * w];

    (* Generate the axis of the screw *)
    axis = Graphics3D[
      Point[q],
      Thickness[0.01 * ScrewSize/2], Line[{q, q + ScrewSize * w}]
    ];

    (* Put everything together as a list of graphics objects *)
    Flatten[{axis, tip} /. {opts} /. Options[DrawScrew]]
  ];

(* Draw a coordinate frame at a point *)
Options[DrawFrame] = {AxisSize->1}

(* Draw a coordinate frame at the appropriate point *)
DrawFrame[p_, R_, opts___Rule] :=
  Flatten[{Graphics3D[
    Thickness[0.01], Line[{p, p + R[[1]] * AxisSize}],
    Line[{p, p + R[[2]] * AxisSize}], Line[{p, p + R[[3]] * AxisSize}]
  ]} /. {opts} /. Options[DrawFrame]];

(*
 * Utility functions for stacking rows, cols, + other matrix operations
 *
 * StackRows		stack rows of matrices together
 * StackCols		stack cols of matrices together
 * zeroMatrix		create a matrix of zeros [private]
 * extractSubMatrix	pick out pieces of a matrix [private]
 *)

(* Stack matrix columns together *)
StackCols[mats__] :=
  Block[
    {i,j},
    Table[
      Join[ Flatten[Table[{mats}[[j]][[i]], {j,Length[{mats}]}], 1] ],
      {i, Length[ {mats}[[1]] ]}]
  ];

(* Stack matrix rows together *)
StackRows[mats__] := Join[Flatten[{mats}, 1]];
	
(* Create matrices of zeros *)
zeroMatrix[nr_, nc_] := Table[0, {nr}, {nc}];
zeroMatrix[nr_] := zeroMatrix[nr, nr];

(* Extract a submatrix from a bigger matrix *)
extractSubMatrix[A_, rows_, cols_] := Map[Take[#, cols]&, Take[A, rows]];


(* Close up the open environments *)
End[];
EndPackage[];

(* End Screws.m *)