maspack.spatialmotion
Class Twist

java.lang.Object
  extended by maspack.matrix.VectorBase
      extended by maspack.spatialmotion.SpatialVector
          extended by maspack.spatialmotion.Twist
All Implemented Interfaces:
java.io.Serializable, java.lang.Cloneable, Vector, Clonable

public class Twist
extends SpatialVector

A spatial vector that represents a spatial velocity, comprised of a translation velocity v and an angular velocity w. A twist is a contravariant spatial vector, with v being the free vector and w being the line vector.

See Also:
Serialized Form

Field Summary
 Vector3d v
          Translational velocity
 Vector3d w
          Rotational (angular) velocity
static Twist ZERO
           
 
Constructor Summary
Twist()
          Creates a Twist and initializes its contents to zero.
Twist(double vx, double vy, double vz, double wx, double wy, double wz)
          Creates a Twist and initializes its components to the specified values.
Twist(Twist tw)
          Creates a Twist and initializes its contents to the values of an existing Twist.
Twist(Vector3d v, Vector3d w)
          Creates a Twist and initializes its components to the specified values.
 
Method Summary
 void absolute(Twist tw1)
          Sets the elements of this twist to their absolute values.
 void add(Twist tw1)
          Adds this twist to tw1 and places the result in this twist.
 void add(Twist tw1, Twist tw2)
          Adds twist tw1 to tw2 and places the result in this twist.
 Twist clone()
           
 void combine(double s1, Twist tw1, double s2, Twist tw2)
          Computes s1 tw1 + s2 tw2 and places the result in this twist.
 void extrapolateTransform(RigidTransform3d X, double h)
          Extrapolates a rigid spatial transformation by an increment corresponding to this Twist scaled by h.
 void extrapolateTransformWorld(RigidTransform3d X, double h)
          Extrapolates a rigid spatial transformation by an increment corresponding to this Twist scaled by h.
 void interpolate(double s, Twist tw1)
          Computes the interpolation (1-s) this + s tw1 and places the result in this twist.
 void interpolate(Twist tw1, double s, Twist tw2)
          Computes the interpolation (1-s) tw1 + s tw2 and places the result in this twist.
 void inverseTransform(RigidTransform3d X, Twist tw1)
          Applies an inverse rigid spatial transformation to the twist tw1, and places the result in this twist.
 void inverseTransform(RotationMatrix3d R, Twist tw1)
          Applies an inverse rotational transformation to the twist tw1, and stores the result in this twist.
 void negate(Twist tw1)
          Sets this twist to the negative of tw1.
 void normalize(Twist tw1)
          Computes a unit twist in the direction of tw1 and places the result in this twist.
 void scale(double s, Twist tw1)
          Scales the elements of twist tw1 by s and places the results in this twist.
 void scaledAdd(double s, Twist tw1)
          Computes s tw1 and adds the result to this twist.
 void scaledAdd(double s, Twist tw1, Twist tw2)
          Computes s tw1 + tw2 and places the result in this twist.
 void set(double vx, double vy, double vz, double wx, double wy, double wz)
          Sets the values of this twist to the specified component values.
 void set(RigidTransform3d X)
          Sets this twist to a representation of a rigid spatial transformation.
 void set(Twist tw)
          Sets the values of this twist to those of twist tw.
 void set(Vector3d v, Vector3d w)
          Sets the values of this twist to the specified component values.
 void setTransform(RigidTransform3d X)
          Sets a rigid spatial transformation to correspond to this Twist.
 void sub(Twist tw1)
          Subtracts tw1 from this twist and places the result in this twist.
 void sub(Twist tw1, Twist tw2)
          Subtracts twist tw1 from tw2 and places the result in this twist.
 void transform(RigidTransform3d X, Twist tw1)
          Applies a rigid spatial transformation to the twist tw1, and places the result in this twist.
 void transform(RotationMatrix3d R, Twist tw1)
          Applies a rotational transformation to the twist tw1 and stores the result in this twist.
 
Methods inherited from class maspack.spatialmotion.SpatialVector
absolute, cross, dot, epsilonEquals, equals, get, get, get, infinityNorm, inverseTransform, inverseTransform, isContravariant, maxElement, minElement, negate, norm, normalize, normSquared, oneNorm, scale, set, set, set, setRandom, setRandom, setRandom, setZero, size, toString, transform, transform
 
Methods inherited from class maspack.matrix.VectorBase
containsNaN, copy, copyAndAdd, copyAndNegate, copyAndScale, copyAndSub, copyAndSubLeft, dot, epsilonEquals, equals, get, getDefaultFormat, hasNaN, isColumnVectorStringsVertical, isFixedSize, isRowVector, scan, set, set, setColumnVectorStringsVertical, setDefaultFormat, setRowVector, setSize, toString, toString, write, write
 
Methods inherited from class java.lang.Object
equals, getClass, hashCode, notify, notifyAll, wait, wait, wait
 

Field Detail

ZERO

public static final Twist ZERO

v

public Vector3d v
Translational velocity


w

public Vector3d w
Rotational (angular) velocity

Constructor Detail

Twist

public Twist()
Creates a Twist and initializes its contents to zero.


Twist

public Twist(Twist tw)
Creates a Twist and initializes its contents to the values of an existing Twist.

Parameters:
tw - twist whose values are to be copied

Twist

public Twist(double vx,
             double vy,
             double vz,
             double wx,
             double wy,
             double wz)
Creates a Twist and initializes its components to the specified values.

Parameters:
vx - translational velocity x component
vy - translational velocity y component
vz - translational velocity z component
wx - rotational velocity x component
wy - rotational velocity y component
wz - rotational velocity z component

Twist

public Twist(Vector3d v,
             Vector3d w)
Creates a Twist and initializes its components to the specified values.

Parameters:
v - translational velocity
w - rotational velocity
Method Detail

setTransform

public void setTransform(RigidTransform3d X)
Sets a rigid spatial transformation to correspond to this Twist.

Parameters:
X - rigid transform
See Also:
extrapolateTransform(maspack.matrix.RigidTransform3d, double)

extrapolateTransform

public void extrapolateTransform(RigidTransform3d X,
                                 double h)
Extrapolates a rigid spatial transformation by an increment corresponding to this Twist scaled by h. The increment is assumed to be in the coordinate frame of the transform, and so this method is equivalent to
 RigidTransform3d Xinc = new RigidTransform3d();
 Twist inc = new Twist();
 inc.scale (h, this);
 Xinc.setTransform (inc);
 X.mul (Xinc);
 

Parameters:
X - rigid transform to extrapolate
h - scaling factor used to compute increment from this twist
See Also:
setTransform(maspack.matrix.RigidTransform3d)

extrapolateTransformWorld

public void extrapolateTransformWorld(RigidTransform3d X,
                                      double h)
Extrapolates a rigid spatial transformation by an increment corresponding to this Twist scaled by h. The increment is assumed to be in a world-aligned frame that shares an origin with the body frame.

Parameters:
X - rigid transform to extrapolate
h - scaling factor used to compute increment from this twist
See Also:
setTransform(maspack.matrix.RigidTransform3d)

set

public void set(double vx,
                double vy,
                double vz,
                double wx,
                double wy,
                double wz)
Sets the values of this twist to the specified component values.

Parameters:
vx - translational velocity x component
vy - translational velocity y component
vz - translational velocity z component
wx - rotational velocity x component
wy - rotational velocity y component
wz - rotational velocity z component

set

public void set(Vector3d v,
                Vector3d w)
Sets the values of this twist to the specified component values.

Parameters:
v - translational velocity
w - rotational velocity

set

public void set(Twist tw)
Sets the values of this twist to those of twist tw.

Parameters:
tw - twist whose values are to be copied

set

public void set(RigidTransform3d X)
Sets this twist to a representation of a rigid spatial transformation. The v field is set to the translation vector, while the w field is set to the rotation axis, scaled by the rotation angle (in radians).

Parameters:
X - rigid transform

add

public void add(Twist tw1,
                Twist tw2)
Adds twist tw1 to tw2 and places the result in this twist.

Parameters:
tw1 - left-hand twist
tw2 - right-hand twist

add

public void add(Twist tw1)
Adds this twist to tw1 and places the result in this twist.

Parameters:
tw1 - right-hand twist

sub

public void sub(Twist tw1,
                Twist tw2)
Subtracts twist tw1 from tw2 and places the result in this twist.

Parameters:
tw1 - left-hand twist
tw2 - right-hand twist

sub

public void sub(Twist tw1)
Subtracts tw1 from this twist and places the result in this twist.

Parameters:
tw1 - right-hand twist

negate

public void negate(Twist tw1)
Sets this twist to the negative of tw1.

Parameters:
tw1 - twist to negate

scale

public void scale(double s,
                  Twist tw1)
Scales the elements of twist tw1 by s and places the results in this twist.

Parameters:
s - scaling factor
tw1 - twist to be scaled

interpolate

public void interpolate(Twist tw1,
                        double s,
                        Twist tw2)
Computes the interpolation (1-s) tw1 + s tw2 and places the result in this twist.

Parameters:
tw1 - left-hand twist
s - interpolation factor
tw2 - right-hand twist

interpolate

public void interpolate(double s,
                        Twist tw1)
Computes the interpolation (1-s) this + s tw1 and places the result in this twist.

Parameters:
s - interpolation factor
tw1 - right-hand twist

scaledAdd

public void scaledAdd(double s,
                      Twist tw1,
                      Twist tw2)
Computes s tw1 + tw2 and places the result in this twist.

Parameters:
s - scaling factor
tw1 - twist to be scaled
tw2 - twist to be added

scaledAdd

public void scaledAdd(double s,
                      Twist tw1)
Computes s tw1 and adds the result to this twist.

Parameters:
s - scaling factor
tw1 - twist to be scaled and added

combine

public void combine(double s1,
                    Twist tw1,
                    double s2,
                    Twist tw2)
Computes s1 tw1 + s2 tw2 and places the result in this twist.

Parameters:
s1 - left-hand scaling factor
tw1 - left-hand twist
s2 - right-hand scaling factor
tw2 - right-hand twist

normalize

public void normalize(Twist tw1)
Computes a unit twist in the direction of tw1 and places the result in this twist.

Parameters:
tw1 - twist to normalize

absolute

public void absolute(Twist tw1)
Sets the elements of this twist to their absolute values.


transform

public void transform(RotationMatrix3d R,
                      Twist tw1)
Applies a rotational transformation to the twist tw1 and stores the result in this twist.

Parameters:
R - rotational transformation matrix
tw1 - twist to transform

inverseTransform

public void inverseTransform(RotationMatrix3d R,
                             Twist tw1)
Applies an inverse rotational transformation to the twist tw1, and stores the result in this twist.

Parameters:
R - rotational transformation matrix
tw1 - twist to transform

transform

public void transform(RigidTransform3d X,
                      Twist tw1)
Applies a rigid spatial transformation to the twist tw1, and places the result in this twist.

Parameters:
X - rigid spatial transformation
tw1 - twist to be transformed

inverseTransform

public void inverseTransform(RigidTransform3d X,
                             Twist tw1)
Applies an inverse rigid spatial transformation to the twist tw1, and places the result in this twist.

Parameters:
X - rigid spatial transformation
tw1 - twist to be transformed

clone

public Twist clone()
Specified by:
clone in interface Clonable
Overrides:
clone in class SpatialVector