RHSRobot:
--------------------------------------------------------------------------
Robot dynamics model.
Each hinge has one degree-of-freedom which may be either revolute or
prismatic. The rotation or translation axis for each hinge is always z.
Additional transformation matrices are defined in the cell array R.
This routine is for a general topological tree with no closed loops.
Branching is allowed. link defines the topological tree. For example
a six link robot might have link = [0 1 2 3 3 3]. This means that
link 1 connects to ground, 0. Link 2 connects to link 1. Link 3
connects to link 2. Links 4, 5 and 6 connect to link 3 so we have
a branch at link 3.
linkOrder relates the link to the state vector.
RHSRobot can take a function for external torques and forces on the
links.
[force, torque] = forceFun( x, t, k, forceData )
where k is the link id
The name of forceFun is the pointer forceFun. forceFun may be empty.
u = frictionFun( v, frictionData ) is the friction function. u is
the joint friction and may be sliding or rotational. cFriction
is an array of data structures for the friction function. u is an
array of friction forces and torques.
u = jointFun( x, jointData ) is the joint torque or force function.
--------------------------------------------------------------------------
Form:
xDot = RHSRobot( x, t, h )
--------------------------------------------------------------------------
------
Inputs
------
x (:,1) State vector [positions or angles;velocities]
t (1,1) Time (passed to forceFun)
h (:,1) Data structure (n links)
.forceFun (1,:) Pointer to the link force and torque function.
.forceData (1,1) Data structure for forceFun
.jointFun (1,:) Pointer to the jointFun
.jointData (1,1) Data structure for the joint function
.frictionFun (1,:) Pointer to the friction function.
.frictionData (1,1) Friction data structure.
.actuatorMap (1,:) Maps the u vector to the links.
.u (:,1) Control vector (of length(h.actuatorMap)
.g0 (3,1) Gravity vector
.name {n} Link name
.inertia {(3,3)} 3-by-3 inertia matrices.
.R {(3,3)} transformation matrix that rotates the hinge axis into z.
.c {(3,1)} vector from the i hinge to the cm in a link.
.r {(3,1)} vector from the i-1 hinge to the i hinge in a link.
.parent (1,:) Parent of the link
.rParent (1,:) Parent hinge vector in the path to this link
.hinge (1,n) Type of hinge 0 for prismatic 1 for revolute
.radius (1,n) Radius of the arm
.mass (1,n) Mass of the link
.stateLinkIndex (1,n) Maps the links to the state vector.
.windVel (3,1) Wind velocity vector
.cP {(3,1}) vector from the cm to center of pressure?
The remaining entries in the data structure h are autogenerated by
the function RHSRobotInitialize
.path {(:)} The link path to each end point
.link (1,n) Previous link.
.linkV (1,n) Velocity source for link.
.linkOrder (1,n) The order of links in the topological tree.
.rMap {(:)} Maps the r vectors to the links
.nB (1,1) Number of branches
.branches {k} Branches per link
.nBranches (:) Number of branches per link
-------
Outputs
-------
xDot (:,1) State derivative vector
massMatrix (:,:) Generalized mass matarix
--------------------------------------------------------------------------
Children:
Math: Linear/Cross
Math: Linear/Skew
