In file /home/blah/darwin2k/src/dyno/mechanism/dyno.h:

class mechanism

Represents a mechanism composed of links (rigid bodies) connected by joints.

Public Fields

[more] Bookkeeping and options
[more] State and dynamics members
[more] Pointers to other entities

Public Methods

[more] mechanism(void)
constructor
[more] ~mechanism(void)
destructor
[more] Kinematics methods
[more] Miscellaneous methods
[more] Dynamics-related methods
[more] Convenience functions

Public Members

[more]enum baseMotionType
Indicates the type of motion for the mechanism's root link.


Documentation

Represents a mechanism composed of links (rigid bodies) connected by joints. A mechanism can compute the forward dynamics, forward kinematics, and differential kinematics (Jacobian) for a connected set of links and joints. The most simple mechanism is a single rigid body; a slightly more complex mechanism would have a series of rigid bodies connected by joints. It is also possible to have branching mechanisms whose graph of links and joints is a tree, and loops can be created from the tree by the addition of constraints.

Creating mechanisms While one could create a mechanism by manually creating links and joints and then adding them to an empty mechanism, it is easier to create an assembly from parts, and then create the mechanism from the assembly through assembly::connectParts(). This will call all the necessary initialization functions and tends to be simpler in general.

Simulating mechanisms After creating a mechanism, you must add it to a dynamicSystem using dynamicSystem::addMechanism() before you can simulation the mechanism's motion. The dynamic system's stepForward() method can then be used to compute the motion of one or more mechanisms over a period of time. Influences, timer, and other components can be added the dynamicSystem as well, allowing complex controllers and physical phenomena to be simulated.

Multiple mechanisms with constraints When constraints between mechanisms are created, one mechanism becomes a parent mechanism and the others become children. The parent mechanism handles the constraint calculations and computation of acceleration for the entire system, since the constraints cause interactions between the mechanisms. This process is mostly transparent to the user.

oenum baseMotionType
Indicates the type of motion for the mechanism's root link.

o Bookkeeping and options

obaseMotionType baseType
the type of motion allowed for the base

oint rootDOFs
the number of DOFs of the root link

oint systemDOFs
the total number of DOFs in this mech and its children

oint stateSize
the dimension of the state vector. This may be different from the number of degrees of freedom: for example, the orientation of a free base is stored as a quaternion, so the state for orientation has 4 entires while orientation has only 3 DOFs.

oint systemStateSize
the size of the system state vector

oint verboseLevel
desired level of output for general mechanism methods

oint dynamicVerboseLevel
desired level of output for methods relating to dynamic simulation

oint inverseIsCurrent
indicates whether Minv is up-to-date

oint usedCS
indicates whether the constraintSolver was used for bilateral constraints

oint recordLinkForces
indicates whether link forces should be recorded for later structural analysis (default is 0)

o State and dynamics members

oint offset
the offset of the mechanism in the dynamic eqns

ovector* systemVel
a pointer to the system velocity vector. This vector is allocated only by parent mechanisms; in child mechanisms, systemVel points to the system velocity vector of the parent.

ovector* systemAcc
a pointer to the system acceleration vector. This vector is allocated only by parent mechanisms; in child mechanisms, systemAcc points to the system acceleration vector of the parent. Additionally, if a parent mechanism has no children, systemAcc points to acc. It is easier for users to use 'acc' to access acceleration than to use systemAcc, though note that updateAccVector() may need to be called in some cases.
See Also:
mechanism::acc
mechanism::updateAccVector

osvTriple* xDotDot
the s-val for the base's acceleration, if the base link is mobile

osvTriple* omegaDot
the s-val for the base's angular acceleration, if the base link is mobile

omatrix M
the mechanism's mass marix

omatrix Minv
the inverse of the mechanism's mass matrix. Check 'inverseIsCurrent' to see if Minv is up-to-date before using it. If it is not, call updateMassInverse().

ovector T
Vector of applied joint torques

ovector V
Vector of 0th and 1st order terms in the dynamics equations

ovector acc
The mechanism's acceleration vector. This may not be up-to-date for mechanisms that have constraints with other mechanisms, in which case you should call updateAccVector() before using 'acc'. The format of acc is:
  • linear acceleration (0 entries for FIXED_BASE, 2 for PLANAR_BASE, or 3 for FREE_BASE
  • angular acceleration (0 entries for FIXED_BASE, 1 for PLANAR_BASE, or 3 for FREE_BASE
  • joint accelerations (one per joint DOF; total number is (numDOFs - (0 for FIXED_BASE, 3 for PLANAR_BASE, 6 for FREE_BASE))

ovector* Ts
The system torque vector for parent mechanisms

omatrix* Ms
The system mass matrix. Only parent mechanisms have this allocated.

omatrix* Minvs
The inverse of the system mass matrix. Only parent mechanisms have this allocated.

ovector* Vs
The system inertial and external force vector. Only parent mechanisms have this allocated.

o Pointers to other entities

omechanism* parent
the parent mechanism (or NULL if this mechanism is a parent)

olink* root
the root link of the mechanism

oassembly* as
the assembly from which the mechanism was built, if any. If 'as' is non-null, it will be deleted when the mechanism is deleted.

odynamicSystem* ds
The dynamicSystem to which the mechanism belongs

oconstraintSolver* cs
The object for solving dynamic constraints. This is only allocated when a mechanism has unilateral constraints.

o mechanism(void)
constructor

o ~mechanism(void)
destructor

o Kinematics methods

oint setBaseType(baseMotionType newType)
sets the type of base motion (fixed, planar, or spatial)

oint setPoseFromJoints(void)
sets the pose of the mechanism's links from joint states

oint setPoseFromJoints(const quaternion &q, const triple &t)
sets the pose of the mechanism's links from joint states, setting the pose of the root link to q & t

oint computeBaseJacobian(const triple &worldPt, matrix &J, int rowOffset, int columnOffset)
computes the jacobian matrix for 'worldPt' with respect to the base's degrees of freedom, and store it in J at the position indicated by (rowOffset, columnOffset)

o Miscellaneous methods

oint checkGraphConsistency(void)
checks link-joint and part-connection graphs for consistency

ovoid nameLinks(void)
allocates names for each link to aid debugging

ovoid print(link* n = NULL)
prints the mechanism's current state

oint setRoot(link* m)
sets the root link of the mechanism

oint isLocked(int sysOffset) const
returns 1 if the indicated DOF is locked, or 0 if not

oinline int* getLock(void) const
gets the array of DOF locking values

ovoid printPose(void)
prints the current pose of the mechanism

oint joinMechanisms(mechanism* otherMech, joint* newJoint)
[NOT IMPLEMENTED] connects otherMech to this one via newJoint

omechanism* split(joint* jt, baseMotionType newBaseMotion)
[NOT IMPLEMENTED] creates a new mechanism by splitting the current mechanism at jt

o Dynamics-related methods

oint getDynamicEquations(matrix* Mm, vector* Vm)
Stores M and V in Mm and Vm, respectively. If Mm (or Vm) is NULL, then M (or V) will not be stored. This method will re-evaluate the dynamic equations if they are not up to date.

oint addConstraintForces(int evaluate, int subtract = 0)
Adds the forces computed for each constraint to Fext and Next of the links they act on. This is useful when the user needs to know the final sum of forces and moments acting on each link, since normally the constraint forces are not actually applied to the mechanism's links--their influence on acceleration is directly computed. If evaluate is 1, then the constant terms of the mechanism's dynamic equations will be re-evaluated. if subtact is -1, then the constraint forces will be subtracted from the system.

oint computeConnectionForces(int addConstraintForces = 1, int evaluateDynamics = 1)
Computes the internal forces at every connection between parts in the mechanism, allowing structural analysis to be performed. If 'addConstraintForces' is non-zero, then mechanism::addConstraintForces() will be called with 'evaluateDynamics' as the first argument.

ovoid printDynamics(int printValue) const
prints the mechanism's dynamics. Prints numeric values if printValue is 1, or symbolic values otherwise

oint updateMassInverse(void)
updates Minv. Does nothing if 'inverseIsCurrent' is non-zero

oinline int updateAccVector(void)
updates 'acc' from systemAcc, if necessary. returns 1 if acc was changes, 0 if not. This method is only needed if you have constraints between mechanisms; otherwise, systemAcc just points to acc.

oint computeAccelerationFromTorque(double* state, double* stateDot, int &appliedImpulse)
Nomputes the acceleration of the system's mechanisms given the applied forces. This method handles constraint satisfaction in addition to the normal dynamic equations. computeAccelerationFromTorque is normally called by the dynamic system, and should only be called for parent mechanisms.

oint setAccelerationFromCommand(double* stateDot)
Sets the system's acceleration to the given value. This method is normally called by the dynamic system, and should only be called for parent mechansims.

oint getState(double* state, double* stateDot)
saves the mechanism's state (and velocity, if stateDot is non-NULL)

oint setState(const double* state, const double* stateDot)
sets the mechanism's position (and velocity, if stateDot is non-NULL)

oint getState(vector &state)
saves the mechanism's state (position and velocity) in the supplied vector, resizing it if necessary to 2*stateSize

oint setState(const vector &state)
sets the mechanism's state (position and velocity) from the supplied vector, returning 0 if the vector is the wrong size or if an error occurred.

oint updateSystemVelFromMech(void)
updates systemVel from link and joint velocities

oint updateMechFromSystemVel(void)
updates link and joint velocities from systemVel

oint computeSystemInverse(void)
computes the inverse mass matrix for the entire system

oint setDynamicValues(void)
updates link velocities and other velocity-dependent variables. Call this after explicitly setting any joint or base velocities to ensure that link velocities get updated.

o Convenience functions

oinline link* getLink(int i)
returns the i'th link

oinline joint* getJoint(int i)
returns the i'th joint

oinline mechanism* getChild(int i)
returns the i'th child mechanism

oinline int numLocked(void) const
returns the number of locked DOFs

oinline int numSystemLocked(void) const
returns the number of locked DOFs in the system

oinline int numLinks(void) const
returns the number of links in the mechanism

oinline int numJoints(void) const
returns the number of links in the mechanism

oinline int numChildren(void) const
returns the number of children in the mechanism

oinline int numBConstraints(void) const
returns the number of bilateral constraints in the system

oinline int numUConstraints(void) const
returns the number of unilateral constraints in the system

oinline int numMyConstraints(void) const
returns the number of this mechanism's constraints


This class has no child classes.
Friends:
dynamicSystem
constraintSolver
constraint
assembly
link
joint

Alphabetic index HTML hierarchy of classes or Java



This page was generated with the help of DOC++.