Kinematics Toolbox: rotparam(r) - File Exchange

Code covered by the BSD License

Highlights from
Kinematics Toolbox

ad(h) AD Performs the adjoint transform
animframetraj(t, scale, folde... ANIMFRAMETRAJ animates a series of frames
arrow3(O, D, color, linewidth) ARROW3 plots a 3d arrow
bjacob(r, theta) BJACOB Calculate the body jacobian for the r
createtwist(omega, q) CREATETWIST Inputs a skew and a point, and returns a twist
dimg(level, name, img) DIMG displays an image if DebugLevel > level
dout(level, varargin) DOUT displays the string if DebugLevel > level
drawframe(r, scale, alternate... DRAWFRAME plots a graphical description of a coordinate frame
drawframediff(t1, t2) DRAWFRAMEDIFF plots the difference between t1 and t2
drawframetraj(t, scale) DRAWFRAMETRAJ plots the a series of homogeneous transforms
drawskew(omega) DRAWSKEW plot a skew's axis of rotation
drawskewtraj(omega, theta) DRAWSKEWTRAJ generates a graphical description of a screw over a series
drawtwist(xi) DRAWTWIST plot a twist's axis of rotation
drawtwisttraj(xi, theta) DRAWTWISTTRAJ generates a graphical description of a twist over a series
dtor(val) DTOR converts degrees to radians
fast_skewexp(s) SKEWEXP Calculate the exponential of a skew-symmetric matrix.
fast_twistexp(xi) TWISTEXP Calculate the exponential of a twist matrix.
fkine(r, theta) FKINE forward kinematics for serial link manipulator
getkeywait(m)
homdiff(ta, tn, method) HOMDIFF Compute differential between two homogeneous transforms in twist
homerror(ta, tn) HOMEERROR calculates the error between two homogeneous transforms
homtotwist(t) HOMTOTWIST finds a twist and a theta to create the homogeneous transform T
iad(xi) IAD Performs the inverse adjoint transform
ikine(rn, pose, x0, tol) IKINE inverse kinematics for serial link manipulator
ikine2(rn, pose, joints, tol) IKINE2 inverse kinematics for serial link manipulator
isequalf(a, b, thresh) ISEQUALF Returns true if the two quantities are equal within a threshold
ishom(t) ISHOM returns true if the matrix is a homogeneous transform
isrobot(r) ISROBOT Returns 1 if the matrix is a robot
isrot(r) ISROT returns true if the matrix is a rotation matrix
isskew(r) ISSKEW returns true if the matrix is a skew-semmetric matrix
istwist(xi) ISTWIST returns true if the matrix is a twist
man(p) MAN emulates man function from the console
named_figure(str, id) NAMED_FIGURE selects a figure based on a string instead of by number
nice3d() NICE3D make 3d plots nicer
noisehom(T, nr, np) NOISEHOM applies uniformly distributed noise to a homogeneous transform
noiseskew(omega, n) NOISESKEW applies uniformly distributed noise to a skew
noisetwist(xi, np, nt) NOISETWIST applies uniformly distributed noise to a twist
pos(x, y, z) POS Set or extract the translational part of a homogeneous matrix
randhom(scale) RANDHOM generates a pseudo-random homogeneous transform
randskew() RANDSKEW generates a pseudo-random skew vector
randtwist(ch) RANDTWIST generates a pseudo-random twist vector
robot(XI, GST, THETA) ROBOT creates a robot struct
robotparams(r) ROBOTPARAMS returns a parameter vector from a robot
rot(x) ROT extracts the rotational part of a homogeneous matrix
rotaxis(R, theta) ROTAXIS calculate the axis of rotation for a matrix R
rotparam(r) ROTPARAM pulls a skew matrix and theta out of a rotation matrix
rotx(phi) ROTX rotate around X by PHI
rotxh(phi) ROTXH return homogeneous rotation matrix around X by PHI
roty(beta) ROTY rotate around Y by BETA
rotyh(beta) ROTY return homogeneous rotation matrix around Y by BETA
rotz(alpha) ROTX rotate around Z by ALPHA
rotzh(alpha) ROTX return homogeneous rotation matrix around Z by ALPHA
rpy(R) RPY returns the X-Y-Z fixed angles of rotation matrix R
rrp() RRP generate a simple roll, roll, prismatic robot
rtod(val) RTOD converts radians to degrees
sjacob(r, theta) SJACOB calculate the spatial jacobian for the robot
skew(w) SKEW generates a skew-symmetric matrix given a vector w
skewcoords(R) SKEWCOORDS generates a vector w given a skew-symmetric matrix R
skewexp(s, theta) SKEWEXP calculate the exponential of a skew-symmetric matrix
skewlog(R) SKEWLOG calculate the log of a rotation matrix
transl(x, y, z) TRANSL set or extract the translational part of a homogeneous matrix
twist(xi) TWIST convert xi from a 6-vector to a 4 x 4 skew-symmetric matrix
twistaxis(xi) TWISTAXIS inputs a twist and returns the axis
twistcoords(xi) TWISTCOORDS convert xi from a 4 x 4 skew symmetric matrix to a 6-vector
twistexp(xi, theta) TWISTEXP calculate the exponential of a twist matrix.
twistlog(h) TWISTLOG calculate the log of a homogeneous transformation
twistmagnitude(xi) TWISTMAGNITUDE inputs a twist and returns the magnitude
twistpitch(xi) TWISTPITCH inputs a twist and returns the pitch
example_kinematics.m
example_robotlinks.m
example_swimmer.m
test_screws.m
example_frame_traj_movie.mpg
swimmer_movie.mpg
View all files

from Kinematics Toolbox by Brad Kratochvil
The kinematics toolbox is intended for prototyping robotics and computer vision related tasks.

rotparam(r)

function [omega theta] = rotparam(r)
%ROTPARAM  pulls a skew matrix and theta out of a rotation matrix
%
%	[OMEGA THETA] = ROTPARAM(R)
%
% Note: This function only returns theta values on the range of 0 to Pi.
%
% See also: ROTAXIS, HOMTOTWIST, SKEWLOG.

% $Id: rotparam.m,v 1.1 2009-03-17 16:40:18 bradleyk Exp $
% Copyright (C) 2005, by Brad Kratochvil

  if ~isrot(r),
    error('SCREWS:rotparam', 'T must be a rotation');
  end
  
  t = (trace(r)-1)/2;
  
  % this is only in case of numerical errors
  if t < -1,
    t = -1;
  elseif t>1,
    t = 1;
  end
  
  % from Murray pg 29-30
  
  theta = acos(t);
  
  omega = rotaxis(r, theta);
  
end

Highlights from Kinematics Toolbox

Highlights from
Kinematics Toolbox