cylinder,
cone,
pyramid,
block,
sphere,
hemisphere,
ellipsoid,
rod,
disk,
plate,
cylinderShell,
coneShell,
sphereShell,
hemisphereShell
Tao Character Dynamics Engine (tao) is a general purpose dynamics library for dynamic modeling, control, and simulation of araticulated branching structures. Branching (tree-like) structures represent a large class of practical systems such as vehicles, animals, and humans. tao provides intuitive, robust, and efficient ways to model, control, and simulate the dynamics of these complex systems. The main motivation embedded tao is part of an ongoing effort to describe the dynamic behavior of branching structures in an accurate and simple form while taking into account the computational efficiency. This manual explains major concepts and algorithms of tao as well as mechanics of how to use tao using examples.
If you have any questions or comments, please contact us at www.arachi.com.
#include "tao.h" |
#define DE_PRECESION_DOUBLE |
There are 4 main modules in tao.
tao library provides extensive matrix and vector routines. Optimized versions of these routines are also available for several platforms. Matrix classes are C++ wrapper classes with underlying routines written in C.
| deChar | char |
| deInt | int |
| deFloat | float or double. If DE_PRECISION_DOUBLE is defined, double is used. |
| deVector3 | 3 x 1 vector. |
| deMatrix3 | 3 x 3 matrix. |
| deQuaternion | 4 x 1 vector. [ Vx, Vy, Vz, W ]. |
| deVector6 | 6 x 1 spatial vector. This consists of two deVector3 vectors. |
| deMatrix6 | 6 x 6 spatial matrix. This consists of four deMatrix3 matrices. |
| deFrame | Homogeneous transformation. This consists of a deQuaterion (rotation) and a deVector3 (translation). This does not support computation of spatial quantities. |
| deTransform | Homogeneous transformation. This consists of a deMatrix3 (rotation) and a deVector3 (translation). This supports computation of spatial quantities, deVector6 and deMatrix6. |
tao provides a small set of dynamics objects that can be extended via inheritance. All coordinates are in the local (body or link) frame except where otherwise specified..
| taoChar | char |
| taoVarSpherical | spherical variable. This provides variables for a spherical joint, position/velocity/acceleration/torque. |
| taoVarDOF1 | one dof variable. This provides variables for a one dof joint, position/velocity/acceleration/torque. |
| taoJointSpherical | spherical (ball) joint. This provides variables for spherical joint inertia/damping/var. |
| taoJointRevolute | revolute (hinge) joint. This provides variables for revolute joint inertia/damping/var. |
| taoJointPrismatic | prismatic (slide) joint. This provides variables for prismatic joint inertia/damping/var. |
| taoNode | unit dynamic object. This provides a rigid body with inertial property. each taoNode is required to have a parent node. each taoNode can multiple taoJoints relating to its parent. |
| taoNodeRoot | root node. This is the root node of a dynamic character. Every character has one taoRootNode. taoRootNode does not have any taoJoint. Each taoRootNode must have a unique id. |
| taoNodeRB | rigid body node. This is the dynamic node of a rigid body. Each taoNodeRB must have a unique id. |
| taoNodePS | particle system node. This is a particle system manager of particles. Each taoNodePS must have a unique id. |
| taoGroup | dynamics group. This set of dynamic characters. This manages a list of taoRootNode. Each taoGroup must have a unique id. |
| taoWorld | dynamics world. This manages a list of taoGroup. |
tao provides a set of control classes.
| taoControlJt | joint space controller. This provides joint space dynamic control. A controller can be attached to a taoRootNode. |
| taoControlOp | operational space controller. This provides operational space dynamic control. A controller can be attached to a taoRootNode. |
| taoControlVh | vehicle controller. This provides vehicle dynamic control. A controller can be attached to a taoRootNode. |
tao can be compiled in debugging mode. In addition, taoLogger class provides simple but effective logging routines.
| taoMassProp | inertial property. This provides arbitrary inertial property, mass/center of mass/inertia tensor. |
| taoLogger | log routine. This provides C++ print routines for logging. Output can be directed to files. |
These classes can be inherited further to define your own classes.
| abstract class | derived classes |
| taoCNode | taoDNode, taoNodeRB, taoNodePS |
| abstract class | derived classes |
| taoDNode | taoNode, taoRootNode |
| taoDVar | taoVarSpherical, taoVarDOF1 |
| taoDJoint | taoJoint |
| taoJoint | taoJointSpherical, taoJointDOF1 |
| taoJointDOF1 | taoJointRevolute, taoJointPrismatic |
| abstract class | derived classes |
| taoABJoint | taoABJointFixed, taoABJointSpherical, taoABJointDOF1 |
| taoABJointDOF1 | taoABJointRevolute, taoABJointPrismatic |
| taoABNode | taoABNodeRoot, taoABNodeNOJ |
| taoABNodeNOJ | taoABNodeNOJ1, taoABNodeNOJn |
| abstract class | derived classes |
| taoControl | taoControlJt, taoControlOp, taoControlVh |
| taoJCParam | taoJCParamSpherical, taoJCParamDOF1 |
| taoJCParamDOF1 | taoJCParamRevolute, taoJCParamPrismatic |
taoNodePS* CreateNodePS(deInt nopc)
{
taoNodePS* psn = new taoNodePS(nopc);
psn->setMass(0.01f);
psn->setCOR(0.5f);
psn->setCOF_dynamic(0.1f);
psn->setCOF_grip(0.1f);
psn->setCOF_static(0.1f);
psn->setCOF_viscous(0.1f);
psn->setRadius(0.02f);
return psn;
}
|
void SetParticlePos(taoNodePS* psn, deInt i, deFrame* global, deVector3* center)
{
psn->position(i)->multiply(global->rotation(), center);
*psn->position(i) += global->translation();
}
|
taoNodeRB* CreateNodeRB(deFrame* global, deInt useBlock, deVector3* min, deVector3* max)
{
deFrame com;
deMassProp mp;
taoNodeRB* rbn = new taoNodeRB;
if (useBlock) {
com.rotation().identity();
com.translation().set(((*min)[0] + (*max)[0]) * 0.5f, ((*min)[1] + (*max)[1]) * 0.5f, ((*min)[2] + (*max)[2]) * 0.5f);
mp.block(DE_DENSITY_WATER, ((*max)[0] - (*min)[0]), ((*max)[1] - (*min)[1]), ((*max)[2] - (*min)[2]), &com);
} else {
*mp.mass() = 1.0f;
mp.center()->zero();
mp.inertia()->identity();
}
rbn->setMass(*mp.mass(), mp.inertia());
rbn->setFrameGraphics(global, mp.center());
rbn->setDampingLinear(0.01f);
rbn->setDampingAngular(0.1f);
rbn->setCOR(0.5f);
rbn->setCOF_dynamic(0.1f);
rbn->setCOF_grip(0.1f);
rbn->setCOF_static(0.1f);
rbn->setCOF_viscous(0.1f);
return rbn;
}
|
The following sample function recursively creates an articulated body tree.
void CreateNodeAB(sceneNode *node, sceneNode *parent)
{
sceneNode* child;
deFrame home, global;
taoNodeRoot* root;
taoNode* obj;
dynamicsData* adata = node->dynamics;
if (adata) {
home = node->GetLocalTransform();
if (parent && parent->dynamics) {
obj = new taoNode(&home, parent->GetTaoNode());
} else { // create a root node
global = node->parent->GetGlobalTransform();
root = new taoNodeRoot(&global);
root->setIsFixed(adata->fixed);
obj = new taoNode(&home, root);
}
SetDynamicsParam(obj, node, adata);
CreateJoints(obj, adata);
obj->addABNode();
}
child = node->child;
while (child) {
CreateNodeAB(child, node);
child = child->sibling;
}
}
|
void SetDynamicsParam(taoNode* obj, dynamicsData* adata)
{
deFrame com;
deMassProp mp;
if (adata->useBlock) {
com.rotation().identity();
com.translation().set(((*adata->min)[0] + (*adata->max)[0]) * 0.5f, ((*adata->min)[1] + (*adata->max)[1]) * 0.5f, ((*adata->min)[2] + (*adata->max)[2]) * 0.5f);
mp.block(adata->density, ((*adata->max)[0] - (*adata->min)[0]), ((*adata->max)[1] - (*adata->min)[1]), ((*adata->max)[2] - (*adata->min)[2]), &com);
} else {
*mp.mass() = adata->mass;
mp.center()->zero();
mp.inertia()->identity();
}
mp.get(obj->mass(), obj->center(), obj->inertia());
obj->setCOR(adata->elasticity);
obj->setCOF_dynamic(adata->dynamicfriction);
obj->setCOF_grip(adata->gripfriction);
obj->setCOF_static(adata->staticfriction);
obj->setCOF_viscous(adata->viscousfriction);
}
|
void CreateJoints(taoNode* obj, dynamicsData *adata)
{
if (adata->prismaticx)
AddJoint(obj, new taoJointPrismatic(TAO_AXIS_X), new taoVarDOF1, 1.0f, 1.0f);
if (adata->prismaticy)
AddJoint(obj, new taoJointPrismatic(TAO_AXIS_Y), new taoVarDOF1, 1.0f, 1.0f);
if (adata->prismaticz)
AddJoint(obj, new taoJointPrismatic(TAO_AXIS_Z), new taoVarDOF1, 1.0f, 1.0f);
if (adata->revolutex)
AddJoint(obj, new taoJointRevolute(TAO_AXIS_X), new taoVarDOF1, 1.0f, 1.0f);
if (adata->revolutey)
AddJoint(obj, new taoJointRevolute(TAO_AXIS_Y), new taoVarDOF1, 1.0f, 1.0f);
if (adata->revolutez)
AddJoint(obj, new taoJointRevolute(TAO_AXIS_Z), new taoVarDOF1, 1.0f, 1.0f);
if (adata->spherical)
AddJoint(obj, new taoJointSpherical, new taoVarSpherical, 1.0f, 1.0f);
}
|
void AddJoint(taoNode* obj, taoJoint* joint, taoDVar* var, deFloat damping, deFloat jointInertia)
{
joint->setDVar(var);
joint->reset();
joint->setDamping(damping);
joint->setInertia(jointInertia);
obj->addJoint(joint);
}
|
taoControl* CreateController(taoNodeRoot* root, deInt controltype, taoControlType lawtype, deFloat kp, deFloat kv)
{
taoControl* controller;
if (controltype == 0) {
controller = new taoControlOp(root);
controller->getControlOp()->_controlLawType = lawtype;
} else if (controltype == 1) {
controller = new taoControlVh(root);
} else {
controller = new taoControlJt(root);
}
controller->getControlJt()->_jcpGlobal->_kp = kp;
controller->getControlJt()->_jcpGlobal->_kv = kv;
controller->getControlJt()->_controlLawType = lawtype;
taoDynamics::initialize(root);
taoDynamics::fwdDynamics(root, root->getGroup()->gravity());
return controller;
}
|
taoGroup* CreateGroup(taoWorld* world, taoNodePS* ps, taoNodeRB* rb, taoNodeRoot* root)
{
taoGroup* group = new taoGroup;
world->addGroup(group, groupid++);
group->addRB(rb, rbid++);
group->addPS(ps, psid++);
group->addRoot(root, rootid++);
group->gravity()->set(0, 0, -10);
return group;
|
taoWorld* CreateWorld()
{
taoWorld* world = new taoWorld;
return world;
}
|
| delete | propagation |
| taoWorld | taoGroup |
| taoGroup | taoNodePS, taoNodeRB, taoNodeRoot |
| taoNodeRoot | taoControl, taoNode |
| taoNode | taoJoint |
| taoJoint | taoDVar |
| class taoControlJt | global joint control parameters |
| _doDynamicControl | turns dynamics control on/off |
| _includeG | turns gravity compensation on/off |
| _controlLawType | control law type such as PD law |
| _jcpGlobal | special taoJCParam for the global control parameters |
| class taoJCParam | local joint control parameters |
| _controlOn | turns joint control on/off |
| _useGlobalGains | turns using global joint position/velocity gains |
| _goalVelocityAuto | turns automatic goal velocity computation on/off |
| _jtLimitSoftOn | turns soft joint limit on/off |
| _jlU | upper joint limit |
| _jlL | lower joint limit |
| _kpU | upper joint limit position gain |
| _kvU | upper joint limit velocity gain |
| _kpL | lower joint limit position gain |
| _kvL | lower joint limit velocity gain |
| _kp | joint position gain |
| _kv | joint velocity gain |
If controller is available, goal position (frame) can be set for individual nodes.
This method extracts out and sets appropriate joint goal position exiting in node.
As an example, if node has only a spherical joint, translational part of
F will not be used.
void taoControlJt::setGoalPosition(taoDNode* node, deFrame* F, deFloat timestamp);
| class taoControlOp | global operational point control parameters |
| _ignoreOC | turns operational space control on/off |
| _doDynamicControl | turns dynamics control on/off |
| _controlLawType | control law type such as PD law |
| class taoOCParam | local operational point control parameters |
| _controlOn | turns joint control on/off |
| _goalVelocityAuto | turns using global joint position/velocity gains |
| _ignore | turns task space control on/off |
| _kp | task space position gain |
| _kv | task space velocity gain |
This method sets position for an operational point.
void taoControlOp::setOpPosition(taoDNode* node, deFrame* nFe);
This method sets goal position for an operational point.
void taoControlOp::setGoalPosition(taoDNode* node, deFrame* F, deFloat timestamp);
This method extracts out and sets appropriate null space joint goal position exiting in \c node.
void setGoalPositionNull(taoDNode* node, deFrame* F, deFloat timestamp);
This chapter describes the tao's built-in funtions for collision detection/resolution systems to use. Using these functions is optional. Please refer to tao Reference Manual : taoCNode, for more information.
deInt taoCNode::impact1(const deVector3* Pie, const deVector3* Ui,
const deFloat cor2, const deFloat cofg2);
This method computes impulse/force and changes velocity/friction using impulse() and force().
deInt taoCNode::impact2(taoCNode* ni, const deVector3* Pie, const deVector3* Ui,
taoCNode* nj, const deVector3* Pje, const deVector3* Uj);
This method computes impulse/force and changes velocity/friction between two
colliding nodes, ni and nj using impact1(), impulse(), and force().
There are four friction coefficients in tao:
Unwanted penetration can be eliminated by using the following method. Essentially, this method provides an effective error correction.
deInt taoCNode::penetration1(const deVector3* Pie, const deVector3* Ui,
const deVector3* pdist, const deFloat dt);
This method computes impulse and changes position/orientation using impulseDist().
void taoCNode::impulseDist(const deVector3* Pie, const deVector3* Yie);
This method changes position and orientation instantaneously by applying the given pseudo impulse.
tao let users to print out information similarly to print functions in stdio.h:
deLogger::Printf(deChar* format, ...); |
As an example, if we like to print out the logging information to a file called "filename", the following should be called:
deLogger::AddOutput(new deLoggerOutputFile(filename)); |
Please refer to tao Reference Manual : deLogger, for more information.
tao provides mass, center of mass, and inertia tensor definitions of homogeneous bodies. Please refer to tao Reference Manual : deMassProp, for more information.
Internal mass properties of deMassProp can be set or retrieved by the following methods:
// set internal values to zero. void zero(); // set internal values to given values void set(const deFloat* mass, const deVector3* center, const deMatrix3* inertia); // get internal values void get(deFloat* mass, deVector3* center, deMatrix3* inertia); |
Methods specifying mass properties for various homogeneous bodies:
cylinder,
cone,
pyramid,
block,
sphere,
hemisphere,
ellipsoid,
rod,
disk,
plate,
cylinderShell,
coneShell,
sphereShell,
hemisphereShell
Note: the calls are accumulative for internal mass, center of mass, and inertia tensor.
Do not forget to initialize before simulation/control loop starts.
void Initialize(taoNodeRoot* root)
{
// for simulation
taoDynamics::initialize(root);
// for control
taoDynamics::fwdDynamics(root, root->getGroup()->gravity());
}
|
tao update should be called at least once each graphics update. taoWorld::Update() can be replaced by following 3 individual call: control(), simulate(), updateTransformation(). time is the desired goal achieving time. this value is used to compute the goal frames. Notice that this value should be greater than the last control time, taoControl::time() and less than equal to the current goal time set by taoControl::setGoalPosition(). dt is the integration time step. notice that this value is independent to time. n is the maximum number of iteration of the loop if necessary (normally, 1).
void Update(taoWorld* world, deFloat time, deInt numIter)
{
static deFloat lasttime = 0;
deInt n = numIter;
deFloat dt = (time - lasttime) / n;
while (n-- > 0)
{
lasttime += dt;
world->update(lasttime, dt, 1);
// this update() can be replaced by following 3 individual call.
//world->control(lasttime);
//world->simulate(dt);
//world->updateTransformation();
// do collision detection/resolution
}
// update the graphics using taoWorld
}
|
For taoNode, both frameGlobal and getFrameGraphics will give the same frame. So, using frameGlobal diretly will be faster. For taoNodeRB, the easiest way to do is to set up the graphics center frame concident with the center of mass frame. This will eliminate the offset between the global frames in graphics and dynamics. In that case, frameGlobal can be used directly and it is faster.
In most cases, it is more efficient and stable to use the same step size for both integration and control steps. In addition, since the step size of control loop dictates the motion and smaller step size gives better result, it is recommended to use smaller stepsize with faster integrator such as explicit Euler integrator.
After integration step, all external force accumulators are reset to zero. Notice that impact1() and impact2() will replace whatever values in external force accumulators with friction forces. Therefore, users should add appropriate external forces to the accumulators between the collision resolution step and the integration step.
Joint constraints are built-in constraints in tao. Since tao uses algorithms using generalized coordinate systems, it computes equations of motions with built-in joint constraints. Notice that since joint constraints are built into the dynamics equations, LCP solver (constraint solver) is not necessary. Joint constraints in tao are infinitely hard constraints.
Soft constraints can be done using appropriate controllers. Specially, tao provides built-in pseudo joint limit constraints via taoControlJt. These joint limits behave as soft constraints. Note that these soft joint limit constraints still do not require LCP solver (constraint solver).
Gravity can be set to each taoGroup anytime and affects all dynamics objects in that taoGroup.
void SetGravity(taoGroup* group, deFloat x, deFloat y, deFloat z)
{
group->gravity()->set(x, y, z);
}
|
Joint velocity can be clamped for stability. Notice that this might generate unpredictable behavior due to physically inconsistent clipping.
void ClampJointVelocity(taoJoint* joint, deFloat x, deInt clampOn, deFloat dQmax)
{
joint->setDQclamp(clampOn);
joint->setDQmax(dQmax);
}
|
tao provides a framework to apply an impulse on a node of dynamics characters on the fly.
void Impulse(taoNode *contactNode, deVector3 *contactPoint, deVector3 *impulse)
{
taoDynamics::impulse(contactNode, contactPoint, impulse);
}
|
tao provides a framework to unlink at a node (subtree) of dynamics characters on the fly.
void Unlink(taoNodeRoot* root, taoNode *dnode, deInt fixed, deFloat time)
{
taoNodeRoot* r;
if (fixed)
r = data->root->getGroup()->unlinkFixed(root, dnode);
else
r = data->root->getGroup()->unlinkFree(root, dnode, 1, 1);
root->getGroup()->sync(root, time);
}
|
tao allows for a node to have multiple joints. In this case the order is important. As an example, for the floating base, 3 prismatic joints must be added to the node before a spherical joint is added. Notice that a node can have the maximum of 6 DOF.
If a node has only one spherical joint, the translation part of localFrame() will be preserved during frame updates. The effect of this procedure is the same as changing the translation part of homeFrame() with the translation part of localFrame().
Since Fl = Fh * Fs = [rh, ph][rs, 0] = [rh*rs, ph] = [rl, pl] and ph should be considered as a constant vector, pl can be set with an arbitrary vector in order to accomodate changes in the translation part.
Notice that if pl != ph, dynamics can be off due to the discontinuity in configuration.
// note: stride is the offset of each vertex vector in vertices
// e.g. 3x1 float vectors -> stride = 12
// e.g. 4x1 float vectors -> stride = 16
hZen CreateZenNode(IZen* zs, VertexVector* vertices, int stride, nrgDynData* udata)
{
hZen id = zs->NewNode(ZEN_HULL);
zs->SetUserData(id, (unsigned int)udata);
zs->SetRadius(id, data->radius);
zs->SetError(id, data->error);
zs->SetVertices(id, (float (*)[3])vertices, nvertices, stride);
zs->EndNode(id);
return id;
}
|
void SyncZenNode(IZen* zs, hZen id, taoCNode* node)
{
deFrame Fg;
node->getFrameGraphics(&Fg);
zs->UpdateTransform(id, Fg.rotation(), Fg.translation());
}
|
// test for collisions and report the results (zen).
// Also provide a simple collision response to colliding objects (tao).
void CollisionTestReportAndRespond()
{
ZEN_Result report;
zs->Collide(&report); // perform collision test
taoCNode* node1, *node2;
deVector3 p1, p2, u1, u2, v;
ZEN_ResultContact *contact;
ZEN_ResultPoint *normal;
for (int j = 0; j < report.numNodePairs(); j++)
{
int ncontacts = report.numContacts(j);
if (ncontacts > 0)
{
node1 = ((nrgDynData*)zs->GetUserData(report.node1ID(j)))->node;
node2 = ((nrgDynData*)zs->GetUserData(report.node2ID(j)))->node;
// this is needed only if node1 is articulated body dynamics node
taoNodeRoot* root = ((nrgDynData*)zs->GetUserData(report.node1ID(j)))->root;
if (root)
taoDynamics::opSpaceInertiaMatrix(root);
// this is needed only if node2 is articulated body dynamics node
root = ((nrgDynData*)zs->GetUserData(report.node2ID(j)))->root;
if (root)
taoDynamics::opSpaceInertiaMatrix(((nrgDynData*)zs->GetUserData(report.node2ID(j)))->root);
for (int i = 0; i < ncontacts; i++)
{
contact = report.Contact(j, i);
normal = report.Normal(j, i);
v.set(contact->point[0], contact->point[1], contact->point[2]);
p1.inversedMultiply(*node1->frameGlobal(), v);
p2.inversedMultiply(*node2->frameGlobal(), v);
v.set((*normal)[0],(*normal)[1],(*normal)[2]);
u1.inversedMultiply(node1->frameGlobal()->rotation(), v);
u2.inversedMultiply(node2->frameGlobal()->rotation(), v);
node1->impact2(node1, &p1, &u1, node2, &p2, &u2);
}
}
}
}
|
1.4.6-NO