screws.m

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

(*
 *	      Screws.m - a screw package for mathematica
 *
 *		    Richard Murray and Sudipto Sur
 *				   
 *	     Division of Engineering and Applied Science
 *		  California Institute of Technology
 *			  Pasadena, CA 91125
 *
 * Screws.m is a Mathematica package for performing screw calculus.
 * It follows the treatment described in _A Mathematical Introduction to
 * Robotic Manipulation_, by R. M. Murray, Z. Li, and S. S. Sastry
 * (CRC Press, 1994).  This package implements screw theory in
 * 3-dimensional Euclidean space (some functions work in n dimensions).
 *
 * Copyright (c) 1994, Richard Murray.
 * Permission is granted to copy, modify and redistribute this file, 
 * provided that this header message is retained.
 *
 * Revision history:
 *
 * Version 1.1 (7 Dec 1991)
 *   Initial implementation (as RobotLinks.m)
 *
 * Version 1.2 (10 Dec 1992)
 *   Removed robot specific code, leaving screw theory operations only
 *   Changed name to Screws.m to reflect new emphasis
 *   Rewrote some functions to use features of Mma more effectively
 *
 * Version 1.2a (30 Jan 1995)
 *   Modified TwistExp, TwistMagnitude and TwistAxis, 10/6/94, Jeff Wendlandt
 *   ...Changed If[w.w != 0, ... to If[(MatchQ[w,{0,0,0}] || .., ...
 *   If[w.w != 0,... does not work when w is a symbolic expression.
 *   
 *)

BeginPackage["Screws`", "Graphics`Shapes`"];

(*
 * Function usage
 *
 * Document all of the functions which are defined in this file.
 * This is the same line that appears in the printed documentation.
 *)

AxisToSkew::usage=Skew::usage=
  "AxisToSkew[w] generates skew symmetric matrix given 3 vector w";

SkewToAxis::usage=UnSkew::usage=
  "SkewToAxis[S] extracts vector w from skewsymmetric matrix S";

SkewExp::usage=
 "SkewExp[w,(theta)] gives the matrix exponential of an axis w.
  Default value of Theta is 1.";

RotationAxis::usage=
  "RotationAxis[R] finds the rotation axis given the rotation matrix R";

RotationQ::usage=
  "RotationQ[m] tests whether matrix m is a rotation matrix";

RPToHomogeneous::usage=
  "RPToHomogeneous[R,p] forms homogeneous matrix from rotation matrix R \
  and position vector p";

TwistExp::usage=
 "TwistExp[xi,(Theta)] gives the matrix exponential of a twist xi.
  Default value of Theta is 1.";

TwistToHomogeneous::usage=
  "TwistToHomogeneous[xi] converts xi from a 6 vector to a 4X4 matrix";

HomogeneousToTwist::usage=
  "HomogeneousToTwist[xi] converts xi from a 4x4 matrix to a 6 vector";

RigidOrientation::usage=
  "RigidOrientation[g] extracts rotation matrix R from g";

RigidPosition::usage=
  "RigidPosition[g] extracts position vector p from g";

RigidTwist::usage=
  "RigidTwist[g] extracts 6 vector xi from g";

TwistPitch::usage=
  "TwistPitch[xi] gives pitch of screw corresponding to a twist";

TwistAxis::usage=
  "TwistAxis[xi] gives axis of screw corresponding to a twist";

TwistMagnitude::usage=
  "TwistMagnitude[xi] gives magnitude of screw corresponding to a twist";

RigidAdjoint::usage=
  "RigidAdjoint[g] gives the adjoint matrix corresponding to g";

RigidInverse::usage=
  "RigidInverse[g] gives the inverse transformation of g";

PointToHomogeneous::usage=
  "PointToHomogeneous[q] gives the homogeneous representation of a point";

VectorToHomogeneous::usage=
  "VectorToHomogeneous[q] gives the homogeneous representation of a vector";

ScrewToTwist::usage=
  "ScrewToTwist[h, q, w] returns the twist coordinates of a screw";

DrawScrew::usage=
  "DrawScrew[q, w, h] generates a graphical description of a screw";

DrawFrame::usage=
  "DrawFrame[p, R] generates a graphical description of a coordinate frame";

ScrewSize::usage=
  "ScrewSize sets the length of a screw for DrawScrew";

AxisSize::usage=
  "AxisSize sets the length of an axis vector for DrawFrame";

(* Utility functions which might be useful in this context *)
StackRows::usage="StackRows[Mat1,Mat2,..] stacks matrix rows together";
StackCols::usage="StackCols[Mat1,Mat2,...] stacks matrix columns together";

(*
 * Error messages
 *
 * Use the Mma error message facility so that we can turn off error
 * messages that we don't want to hear about
 *
 *)

AxisToSkew::wrongD = "`1` is not a 3 vector.";
SkewToAxis::notskewsymmetric = "`1` is not a skew symmetric matrix";
Screws::wrongDimensions = "`1`: Dimensions of input matrices incorrect.";
Screws::notSquare = "`1`: Input matrix is not square.";
Screws::notVector = "`1`: Input is not a vector.";

(* Begin private section of the package *)
Begin["`Private`"];

(*
 * Rotation matrices
 *
 * Operations on SO(n), the Lie group of rotation matrices.
 *
 *)

(* Find the axis of a rotation matrix *)
RotationAxis[R_] :=
  Module[
    {v, nr, nc, axis},

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

    (* Construct a dummy vector to operate on *)   
    v = Table[Unique["v"], {nc}];

    (* First check for degenerate case: R = identity *)
    If[And @@ (R . v == v), 
        Message[Screws::notUnique, "RotationAxis"];
        axis = Table[0, {nc}];  axis[[1]] = 1;
        Return[axis];
    ];

    (* Otherwise, solve a linear equation to find the answer *)
    v /. Solve[R . v == v, v][[1]] /. Map[# -> 1&, v]
  ];

(* Generate a skew symmetric matrix from an axis *)
(*! This only works in R^3 for now !*)
Skew[w_] := AxisToSkew[w];	(* backwards compatibility *)
AxisToSkew[omega_?VectorQ]:=
  Module[
    {},

    (* Check to make sure the dimensions are okay *)
    If[Not[VectorQ[omega]] || Length[omega] != 3,
      Message[Screws::wrongDimension];
      Return Null;
    ];

    (* Return the appropriate matrix *)
    {{ 0,          -omega[[3]],  omega[[2]]},
     { omega[[3]], 0,           -omega[[1]]},
     {-omega[[2]], omega[[1]],  0          }}
  ];

(* Generate an axis from a skew symmetric matrix *)
UnSkew[S_] := SkewToAxis[S];	(* for compatibility *)
SkewToAxis[S_]:=
  Module[
    {},

    (* First check to make sure we have a skew symmetric matrix *)
    If[Not[skewQ[S]] || Dimensions[S] != {3,3},
      Message[Screws::wrongDimension];
      Return Null
    ];

    (* Now extract the appropriate component *)
    {S[[3,2]], S[[1,3]], S[[2,1]]}
  ];

(* Matrix exponential for a skew symmetric matrix *)
SkewExp[v_?VectorQ,theta_:1] := SkewExp[AxisToSkew[v],theta];
SkewExp[S_?skewQ,theta_:1]:=
  Module[
    {n = Dimensions[S][[1]]},

    (* Use Rodrigues's formula *)
    IdentityMatrix[3] + Sin[theta] S + (1 - Cos[theta]) S.S
  ];

(*
 * Homogeneous representation
 *
 * These functions convert back and forth from elements of SE(3)
 * and their homogeneous representations.
 *
 *)

(* Convert a rotation + translation to a homogeneous matrix *)
RPToHomogeneous[R_, p_] :=
  Module[
    {n},

    (* Check to make sure the dimensions of the args make sense *)
    If[Not[VectorQ[p]] || Not[MatrixQ[R]] ||
       (n = Length[p]; Dimensions[R] != {n, n}),
	Message[Screws::wrongDimensions, "RPToHomogeneous:"];
    ];

    (* Now put everything together into a homogeneous transformation *)
    StackCols[
      StackRows[R, zeroMatrix[1, n]],
      StackRows[p, {1}]
    ]
  ];  

(* Convert a point into homogeneous coordinates *)
PointToHomogeneous[p_] :=
  Block[{},
    (* Check to make sure the dimensions of the args make sense *)
    If[Not[VectorQ[p]], Message[Screws::notVector, "PointToHomogeneous"]];

    (* Now put everything together into a homogeneous vector *)
    StackRows[p, {1}]
  ];  

(* Convert a vector into homogeneous coordinates *)
VectorToHomogeneous[p_] :=
  Block[{},
    (* Check to make sure the dimensions of the args make sense *)
    If[Not[VectorQ[p]], Message[Screws::notVector, "VectorToHomogeneous"]];

    (* Now put everything together into a homogeneous vector *)
    StackRows[p, {0}]
  ];

(* Extract the orientation portion from a homogeneous transformation *)
RigidOrientation[g_?MatrixQ]:=
  Module[
    {nr, nc},

    (* Check to make sure that we were passed a square matrix *)
    If[Not[MatrixQ[g]] || ({nr, nc} = Dimensions[g]; nr != nc) || nr < 3,
        Message[Screws::wrongDimensions, "RigidOrientation"];
	Return Null;
    ];

    (* Extract the upper left corner *)
    extractSubMatrix[g, {1,nr-1}, {1,nc-1}]
  ];

(* Extract the orientation portion from a homogeneous transformation *)
RigidPosition[g_?MatrixQ]:=
  Module[
    {nr, nc},

    (* Check to make sure that we were passed a square matrix *)
    If[Not[MatrixQ[g]] || ({nr, nc} = Dimensions[g]; nr != nc) || nr < 3,
        Message[Screws::wrongDimensions, "RigidPosition"];
	Return Null;
    ];

    (* Extract the upper left column *)
    Flatten[extractSubMatrix[g, {1,nr-1}, {nc}]]
  ];

(* 
 * Twists
 *
 * Functions for manipulating twists and converting rigid body
 * transformations back and forth from twists.
 *
 *)

(* Figure out the dimension of a twist [private] *)
xidim[xi_?VectorQ] :=
  Module[
    {l = Length[xi], n},

    (* Check the dimensions of the vector to make sure everything is okay *)
    n = (Sqrt[1 + 8l] - 1)/2;
    If[Not[IntegerQ[n]],
      Message[Screws::wrongDimensions, "xidim"];
      Return 0;
    ];
    n
];