00001
00002 #ifndef INCLUDED_KinematicJoint_h_
00003 #define INCLUDED_KinematicJoint_h_
00004
00005 #include "SensorInfo.h"
00006
00007 #include "Shared/plist.h"
00008 #include "Shared/plistSpecialty.h"
00009 #include "Shared/fmatSpatial.h"
00010 #include "Shared/BoundingBox.h"
00011
00012 #include "Planners/PlannerObstacles.h"
00013
00014 #include <set>
00015 #include <algorithm>
00016
00017 class KinematicJoint;
00018 class IKSolver;
00019
00020
00021
00022 class LinkComponent : virtual public plist::Dictionary {
00023 friend class KinematicJoint;
00024
00025 public:
00026
00027 LinkComponent()
00028 : plist::Dictionary(), mass(0), centerOfMass(), visible(true), material(),
00029 model("CollisionModel"), modelScale(1,1,1), modelRotation(), modelOffset(),
00030 collisionModel(), collisionModelScale(1,1,1), collisionModelRotation(), collisionModelOffset(),
00031 parent(NULL),
00032 bbDirty(false), boundingBox(),
00033 collisionModelListener(collisionModel,*this,&LinkComponent::dirtyBB,false),
00034 collisionModelScaleListener(collisionModelScale,*this,&LinkComponent::dirtyBB,false),
00035 collisionModelRotationListener(collisionModelRotation,*this,&LinkComponent::dirtyBB,false),
00036 collisionModelOffsetListener(collisionModelOffset,*this,&LinkComponent::dirtyBB,false)
00037 {
00038 init();
00039 }
00040
00041
00042 LinkComponent(const LinkComponent& c)
00043 : plist::Dictionary(), mass(c.mass), centerOfMass(c.centerOfMass), visible(c.visible), material(c.material),
00044 model(c.model), modelScale(c.modelScale), modelRotation(c.modelRotation), modelOffset(c.modelOffset),
00045 collisionModel(c.collisionModel), collisionModelScale(c.collisionModelScale), collisionModelRotation(c.collisionModelRotation), collisionModelOffset(c.collisionModelOffset),
00046 parent(NULL),
00047 bbDirty(c.bbDirty), boundingBox(c.boundingBox),
00048 collisionModelListener(collisionModel,*this,&LinkComponent::dirtyBB,false),
00049 collisionModelScaleListener(collisionModelScale,*this,&LinkComponent::dirtyBB,false),
00050 collisionModelRotationListener(collisionModelRotation,*this,&LinkComponent::dirtyBB,false),
00051 collisionModelOffsetListener(collisionModelOffset,*this,&LinkComponent::dirtyBB,false)
00052 {
00053 init();
00054 }
00055
00056
00057 LinkComponent& operator=(const LinkComponent& link) {
00058 if(&link==this)
00059 return *this;
00060 mass = link.mass;
00061 centerOfMass = link.centerOfMass;
00062 visible = link.visible;
00063 material = link.material;
00064 model = link.model;
00065 modelScale = link.modelScale;
00066 modelRotation = link.modelRotation;
00067 modelOffset = link.modelOffset;
00068 collisionModel = link.collisionModel;
00069 collisionModelScale = link.collisionModelScale;
00070 collisionModelRotation = link.collisionModelRotation;
00071 collisionModelOffset = link.collisionModelOffset;
00072
00073 bbDirty = link.bbDirty;
00074 boundingBox = link.boundingBox;
00075 return *this;
00076 }
00077
00078 virtual ~LinkComponent() {}
00079
00080 plist::Primitive<float> mass;
00081 plist::Point centerOfMass;
00082
00083 plist::Primitive<bool> visible;
00084
00085 plist::Primitive<std::string> material;
00086
00087 plist::Primitive<std::string> model;
00088 plist::Point modelScale;
00089 plist::Point modelRotation;
00090 plist::Point modelOffset;
00091
00092 plist::Primitive<std::string> collisionModel;
00093 plist::Point collisionModelScale;
00094 plist::Point collisionModelRotation;
00095 plist::Point collisionModelOffset;
00096
00097
00098
00099 PlannerObstacle2D* getObstacle(const fmat::Transform& worldT) const;
00100
00101
00102 void getObstacles(const fmat::Transform& worldT, HierarchicalObstacle& obs, bool recurse) const;
00103
00104
00105 virtual BoundingBox3D getOwnAABB() const { return getAABB(); }
00106
00107
00108 const BoundingBox3D& getAABB() const { if(bbDirty) updateBB(); return boundingBox; }
00109
00110
00111
00112 bool getOwnBB2D(const fmat::Transform& worldT, RectangularObstacle& ro) const;
00113
00114
00115
00116 bool getOwnBB3D(const fmat::Transform& worldT, BoxObstacle& bo) const;
00117
00118
00119
00120
00121 bool getBB2D(const fmat::Transform& worldT, RectangularObstacle& ro) const;
00122
00123
00124
00125
00126 bool getBB3D(const fmat::Transform& worldT, BoxObstacle& bo) const;
00127
00128
00129
00130
00131 fmat::Column<4> getMassVector() const { return fmat::pack(mass*centerOfMass[0],mass*centerOfMass[1],mass*centerOfMass[2],mass); }
00132
00133
00134 virtual void sumLinkCenterOfMass(fmat::Column<3>& cOfM, float& totalMass) const;
00135
00136
00137
00138 virtual fmat::Column<4> sumLinkCenterOfMass() const { return getMassVector(); }
00139
00140
00141 virtual bool hasMass() const { return mass>0; }
00142
00143
00144 void getModelTransform(fmat::Transform& tr) const;
00145
00146
00147 fmat::Transform getModelTransform() const {
00148 fmat::Transform tr; getModelTransform(tr); return tr;
00149 }
00150
00151
00152 void getCollisionModelTransform(fmat::Transform& tr) const;
00153
00154
00155 fmat::Transform getCollisionModelTransform() const {
00156 fmat::Transform tr; getCollisionModelTransform(tr); return tr;
00157 }
00158
00159
00160 KinematicJoint* getParent() const { return parent; }
00161
00162
00163 KinematicJoint* getNextMobileAncestor() const;
00164
00165 PLIST_CLONE_DEF(LinkComponent,new LinkComponent(*this));
00166
00167 protected:
00168 KinematicJoint* parent;
00169
00170 mutable bool bbDirty;
00171 mutable BoundingBox3D boundingBox;
00172
00173 virtual void dirtyBB();
00174 virtual void updateBB() const;
00175 void computeOwnAABB(BoundingBox3D& bb) const;
00176 static void computeBB2D(const fmat::Transform& fullT, RectangularObstacle& ro, const fmat::Column<3>& obD);
00177
00178 void init() {
00179 addEntry("Mass",mass,"Mass of the component, in kilograms. If 0, indicates static object, does not move but can still collide. (default 0)");
00180 addEntry("CenterOfMass",centerOfMass,"Position of average mass relative to parent frame.");
00181 addEntry("Visible",visible,"If true, indicates component should be rendered for simulated cameras, otherwise only appears in the Mirage user window. (default true)");
00182 addEntry("Material",material,"Name of an Ogre material, found in mirage/media/*.material files.");
00183 addEntry("Model",model,"An Ogre .mesh file, or \"CollisionModel\" to render the collision primitive. (default CollisionModel)");
00184 addEntry("ModelScale",modelScale,"Scales the graphics mesh loaded by Model.");
00185 addEntry("ModelRotation",modelRotation,"Rotates the graphics mesh loaded by Model, relative to parent frame. (supply axis component of quaternion, e.g. from axis/angle: axis * sin(angle/2) )");
00186 addEntry("ModelOffset",modelOffset,"Positions the graphics mesh loaded by Model, relative to parent frame.");
00187 addEntry("CollisionModel",collisionModel,"A Bullet primitive collision shape: { Cube | Cylinder | Sphere | Plane }");
00188 addEntry("CollisionModelScale",collisionModelScale,"Scales the CollisionModel, which by default is 1x1x1, so this sets the object dimensions.");
00189 addEntry("CollisionModelRotation",collisionModelRotation,"Rotates the CollisionModel relative to parent frame. (supply axis component of quaternion, e.g. from axis/angle: axis * sin(angle/2) )");
00190 addEntry("CollisionModelOffset",collisionModelOffset,"Positions the CollisionModel, relative to parent frame.");
00191 setLoadSavePolicy(FIXED,SYNC);
00192 }
00193
00194
00195 plist::PrimitiveCallbackMember<LinkComponent> collisionModelListener;
00196 plist::CollectionCallbackMember<LinkComponent> collisionModelScaleListener;
00197 plist::CollectionCallbackMember<LinkComponent> collisionModelRotationListener;
00198 plist::CollectionCallbackMember<LinkComponent> collisionModelOffsetListener;
00199 };
00200
00201
00202
00203 class KinematicJoint : public virtual plist::PrimitiveListener, public LinkComponent {
00204 friend class KinematicJointLoader;
00205 friend class KinematicJointSaver;
00206 public:
00207 class BranchListener : public plist::Listener {
00208 public:
00209 virtual void kinematicJointBranchAdded(KinematicJoint& parent, KinematicJoint& branch) {};
00210 virtual void kinematicJointBranchRemoved(KinematicJoint& parent, KinematicJoint& branch) {};
00211 virtual void kinematicJointReconfigured(KinematicJoint& joint) {};
00212 };
00213
00214
00215 KinematicJoint() :
00216 plist::Dictionary(), LinkComponent(),
00217 jointType(REVOLUTE, jointTypeNames), theta(0), d(0), alpha(0), r(0), qOffset(0), qmin(0), qmax(0),
00218 components(), frictionForce(0.5f), anistropicFrictionRatio(1,1,1), ikSolver(), sensorInfo(),
00219 controllerInfo(), outputOffset(), branches(), branchListeners(NULL),
00220 depth(0), q(0), To(), Tq(), ik(NULL), componentsListener(components,*this)
00221 {
00222 initEntries();
00223 }
00224
00225
00226 KinematicJoint(const KinematicJoint& kj) :
00227 plist::Dictionary(), LinkComponent(kj),
00228 jointType(kj.jointType), theta(kj.theta), d(kj.d), alpha(kj.alpha), r(kj.r),
00229 qOffset(kj.qOffset), qmin(kj.qmin), qmax(kj.qmax),
00230 components(kj.components), frictionForce(kj.frictionForce),
00231 anistropicFrictionRatio(kj.anistropicFrictionRatio), ikSolver(kj.ikSolver),
00232 sensorInfo(kj.sensorInfo), controllerInfo(kj.controllerInfo), outputOffset(kj.outputOffset),
00233 branches(), branchListeners(NULL), depth(0), q(kj.q), To(kj.To), Tq(kj.Tq),
00234 ik(NULL), componentsListener(components,*this)
00235 {
00236 initEntries();
00237 copyBranches(kj);
00238 }
00239
00240 KinematicJoint& operator=(const KinematicJoint& kj) {
00241 shallowCopy(kj);
00242 copyBranches(kj);
00243 return *this;
00244 }
00245
00246 void shallowCopy(const KinematicJoint& kj) {
00247 if(&kj==this)
00248 return;
00249 LinkComponent::operator=(kj);
00250 jointType = kj.jointType;
00251 theta = kj.theta;
00252 d = kj.d;
00253 alpha = kj.alpha;
00254 r = kj.r;
00255 qOffset = kj.qOffset;
00256 qmin = kj.qmin;
00257 qmax = kj.qmax;
00258 components = kj.components;
00259 frictionForce=kj.frictionForce;
00260 anistropicFrictionRatio=kj.anistropicFrictionRatio;
00261 ikSolver = kj.ikSolver;
00262 sensorInfo = kj.sensorInfo;
00263 controllerInfo = kj.controllerInfo;
00264 outputOffset = kj.outputOffset;
00265 q = kj.q;
00266 To = kj.To;
00267 Tq = kj.Tq;
00268 fireReconfigured();
00269
00270
00271
00272
00273
00274 }
00275
00276 void copyBranches(const KinematicJoint& kj) {
00277 if(&kj==this)
00278 return;
00279
00280 std::set<KinematicJoint*> newBranches;
00281 for(std::set<KinematicJoint*>::const_iterator it=kj.branches.begin(); it!=kj.branches.end(); ++it)
00282 newBranches.insert(new KinematicJoint(**it));
00283 clearBranches();
00284 for(std::set<KinematicJoint*>::const_iterator it=newBranches.begin(); it!=newBranches.end(); ++it)
00285 addBranch(*it);
00286 }
00287
00288 virtual ~KinematicJoint();
00289
00290
00291 enum JointType_t {
00292 REVOLUTE,
00293 PRISMATIC
00294 };
00295
00296 static const char* jointTypeNames[3];
00297
00298 plist::NamedEnumeration<JointType_t> jointType;
00299
00300
00301 plist::Angle theta;
00302 plist::Primitive<fmat::fmatReal> d;
00303 plist::Angle alpha;
00304 plist::Primitive<fmat::fmatReal> r;
00305 plist::Angle qOffset;
00306
00307 plist::Angle qmin;
00308 plist::Angle qmax;
00309
00310
00311 plist::ArrayOf<LinkComponent> components;
00312 typedef plist::ArrayOf<LinkComponent>::const_iterator component_iterator;
00313
00314 plist::Primitive<float> frictionForce;
00315 plist::Point anistropicFrictionRatio;
00316
00317 plist::Primitive<std::string> ikSolver;
00318
00319 plist::ArrayOf<SensorInfo> sensorInfo;
00320
00321
00322
00323 struct ControllerInfo : virtual public plist::Dictionary {
00324 explicit ControllerInfo() : plist::Dictionary(), velocity(false), forceControl(false) { init(); }
00325 ControllerInfo(const ControllerInfo& ci) : plist::Dictionary(), velocity(ci.velocity), forceControl(ci.forceControl) { init(); }
00326 plist::Primitive<bool> velocity;
00327 plist::Primitive<bool> forceControl;
00328 protected:
00329
00330 void init() {
00331 addEntry("Velocity",velocity,"Adjusts interpretation of feedback and application of friction, false implies position control");
00332 addEntry("ForceControl",forceControl,"If true, simulation will use force control to move the joint rather than using position constraints. Grippers should set this to true for more realistic object interaction.");
00333 }
00334 };
00335 ControllerInfo controllerInfo;
00336
00337
00338 plist::OutputSelector outputOffset;
00339
00340 void getObstacles(const fmat::Transform& worldT, class HierarchicalObstacle& obs, bool recurse) const;
00341
00342
00343
00344 virtual BoundingBox3D getOwnAABB() const { BoundingBox3D bb; computeOwnAABB(bb); return bb; }
00345
00346
00347 float getQ() const { return q; }
00348
00349
00350 void setQ(float x) { if(x!=q) { q=x; updateTq(); } }
00351
00352
00353 bool tryQ(float x);
00354
00355
00356 void freezeQ(float x) { qmin=qmax=x; setQ(x); }
00357
00358
00359 bool validQ(float x) const { return (x>=qmin && x<=qmax); }
00360
00361
00362
00363
00364 template<class M> void pushChildrenQIntoArray(M& values, int deoffset, unsigned int max) const {
00365 if(outputOffset!=plist::OutputSelector::UNUSED && static_cast<unsigned int>(outputOffset-deoffset)<max)
00366 values[outputOffset-deoffset]=q;
00367 for(std::set<KinematicJoint*>::const_iterator it=branches.begin(); it!=branches.end(); ++it)
00368 (*it)->pushChildrenQIntoArray(values, deoffset, max);
00369 }
00370
00371
00372
00373
00374 template<class M> void pushAncestorsQIntoArray(M& values, int deoffset, unsigned int max) const {
00375 if(outputOffset!=plist::OutputSelector::UNUSED && static_cast<unsigned int>(outputOffset-deoffset)<max)
00376 values[outputOffset-deoffset]=q;
00377 if(parent!=NULL)
00378 parent->pushAncestorsQIntoArray(values, deoffset, max);
00379 }
00380
00381
00382
00383
00384 template<class M> void pullChildrenQFromArray(M& values, int deoffset, unsigned int max) {
00385 if(outputOffset!=plist::OutputSelector::UNUSED && qmin!=qmax && static_cast<unsigned int>(outputOffset-deoffset)<max)
00386 setQ(values[outputOffset-deoffset]);
00387 for(std::set<KinematicJoint*>::const_iterator it=branches.begin(); it!=branches.end(); ++it)
00388 (*it)->pullChildrenQFromArray(values, deoffset, max);
00389 }
00390
00391
00392
00393
00394 template<class M> void pullAncestorsQFromArray(M& values, int deoffset, unsigned int max) {
00395 if(outputOffset!=plist::OutputSelector::UNUSED && qmin!=qmax && static_cast<unsigned int>(outputOffset-deoffset)<max)
00396 setQ(values[outputOffset-deoffset]);
00397 if(parent!=NULL)
00398 parent->pullAncestorsQFromArray(values, deoffset, max);
00399 }
00400
00401
00402 void zeroChildrenQ();
00403
00404
00405 void zeroAncestorQ();
00406
00407
00408
00409 fmat::Transform getFullT() const;
00410
00411
00412 fmat::Transform getFullInvT() const { return getFullT().rigidInverse(); }
00413
00414
00415 fmat::Transform getT(const KinematicJoint& j) const;
00416
00417
00418
00419 const fmat::Transform& getTo() const { return To; }
00420
00421
00422
00423 const fmat::Transform& getTq() const { return Tq; }
00424
00425
00426 void sumCenterOfMass(fmat::Column<3>& cOfM, float& totalMass) const;
00427
00428
00429
00430 fmat::Column<4> sumCenterOfMass() const;
00431
00432
00433
00434 virtual fmat::Column<4> sumLinkCenterOfMass() const;
00435
00436 using LinkComponent::sumLinkCenterOfMass;
00437
00438 virtual bool hasMass() const;
00439
00440
00441
00442
00443 fmat::Column<6> getJointJacobian(const fmat::Column<3>& p) const;
00444
00445
00446
00447
00448 void getFullJacobian(const fmat::Column<3>& p, std::vector<fmat::Column<6> >& j) const {
00449 if(parent!=NULL)
00450 parent->getFullJacobian(p,j);
00451 j.push_back(getJointJacobian(p));
00452 }
00453
00454
00455
00456
00457 void getMobileJacobian(const fmat::Column<3>& p, std::vector<fmat::Column<6> >& j) const {
00458 if(parent!=NULL)
00459 parent->getMobileJacobian(p,j);
00460 if(isMobile())
00461 j.push_back(getJointJacobian(p));
00462 }
00463
00464
00465 fmat::Column<3> getWorldPosition() const { return getFullT().translation(); }
00466
00467 fmat::Matrix<3,3> getWorldRotation() const { return getFullT().rotation(); }
00468
00469
00470 fmat::Column<3> getPosition() const { return Tq.translation(); }
00471
00472 fmat::Column<3> getParentPosition() const;
00473
00474 fmat::Matrix<3,3> getRotation() const { return Tq.rotation(); }
00475
00476
00477 fmat::Quaternion getWorldQuaternion() const;
00478
00479 fmat::Quaternion getQuaternion(const KinematicJoint& j) const;
00480
00481 fmat::Quaternion getQuaternion() const { return fmat::Quaternion::fromMatrix(Tq.rotation()); }
00482
00483
00484 virtual void loadXML(xmlNode* node);
00485
00486 virtual void saveXML(xmlNode* node, bool onlyOverwrite, std::set<std::string>& seen) const;
00487 using plist::Dictionary::saveXML;
00488
00489
00490
00491
00492
00493
00494 template<class M> void buildChildMap(M& childMap, int deoffset, unsigned int max) {
00495 if(outputOffset!=plist::OutputSelector::UNUSED &&
00496 static_cast<unsigned int>(outputOffset-deoffset)<max && !childMap[outputOffset-deoffset])
00497 childMap[outputOffset-deoffset] = this;
00498 for(std::set<KinematicJoint*>::const_iterator it=branches.begin(); it!=branches.end(); ++it)
00499 (*it)->buildChildMap(childMap,deoffset,max);
00500 }
00501
00502
00503
00504
00505
00506 template<class M> void buildChildMap(M& childMap, int deoffset, unsigned int max) const {
00507 if(outputOffset!=plist::OutputSelector::UNUSED &&
00508 static_cast<unsigned int>(outputOffset-deoffset)<max && !childMap[outputOffset-deoffset])
00509 childMap[outputOffset-deoffset] = this;
00510 for(std::set<KinematicJoint*>::const_iterator it=branches.begin(); it!=branches.end(); ++it)
00511 (*it)->buildChildMap(childMap,deoffset,max);
00512 }
00513
00514
00515
00516
00517
00518 template<class M> void buildMobileChildMap(M& childMap, int deoffset, unsigned int max) {
00519 if(isMobile() && outputOffset!=plist::OutputSelector::UNUSED && static_cast<unsigned int>(outputOffset-deoffset)<max)
00520 childMap[outputOffset-deoffset] = this;
00521 for(std::set<KinematicJoint*>::const_iterator it=branches.begin(); it!=branches.end(); ++it)
00522 (*it)->buildMobileChildMap(childMap,deoffset,max);
00523 }
00524
00525
00526
00527
00528
00529 template<class M> void buildMobileChildMap(M& childMap, int deoffset, unsigned int max) const {
00530 if(isMobile() && outputOffset!=plist::OutputSelector::UNUSED && static_cast<unsigned int>(outputOffset-deoffset)<max)
00531 childMap[outputOffset-deoffset] = this;
00532 for(std::set<KinematicJoint*>::const_iterator it=branches.begin(); it!=branches.end(); ++it)
00533 (*it)->buildMobileChildMap(childMap,deoffset,max);
00534 }
00535
00536
00537 typedef std::set<KinematicJoint*>::const_iterator branch_iterator;
00538
00539
00540 const std::set<KinematicJoint*>& getBranches() const { return branches; }
00541
00542
00543
00544 KinematicJoint* addBranch(KinematicJoint* b);
00545
00546
00547
00548 KinematicJoint* removeBranch(KinematicJoint* b);
00549
00550
00551 void clearBranches();
00552
00553
00554 bool isBranch() const { return branches.size()>1; }
00555
00556
00557
00558
00559 KinematicJoint* cloneBranch() const;
00560
00561
00562
00563 KinematicJoint* nextJoint() const { return *branches.begin(); }
00564
00565
00566 bool isMobile() const { return qmin!=qmax; }
00567
00568
00569 KinematicJoint* getRoot() { KinematicJoint* kj=this; while(kj->parent!=NULL) kj=kj->parent; return kj; }
00570
00571 const KinematicJoint* getRoot() const { const KinematicJoint* kj=this; while(kj->parent!=NULL) kj=kj->parent; return kj; }
00572
00573 std::string getPath(const std::string& sep = "/") const { return (parent==NULL ? std::string() : parent->getPath(sep)) + sep + outputOffset.get(); }
00574
00575 size_t getDepth() const { return depth; }
00576
00577 void addBranchListener(BranchListener* l) const;
00578 void removeBranchListener(BranchListener* l) const;
00579 bool isBranchListener(BranchListener* l) const;
00580
00581
00582 IKSolver& getIK() const;
00583
00584 PLIST_CLONE_DEF(KinematicJoint,new KinematicJoint(*this));
00585
00586 protected:
00587
00588 void initEntries() {
00589
00590
00591
00592 addEntry("JointType",jointType);
00593 addEntry("θ",theta);
00594 addEntry("d",d);
00595 addEntry("α",alpha);
00596 addEntry("r",r);
00597 addEntry("qOffset",qOffset);
00598 addEntry("Min",qmin);
00599 addEntry("Max",qmax);
00600 addEntry("Components",components);
00601 addEntry("FrictionForce",frictionForce);
00602 addEntry("AnistropicFrictionRatio",anistropicFrictionRatio);
00603 addEntry("IKSolver",ikSolver);
00604 addEntry("SensorInfo",sensorInfo);
00605 addEntry("ControllerInfo",controllerInfo);
00606 addEntry("Name",outputOffset);
00607 outputOffset.setRange(0,-1U);
00608 addSelfListener();
00609 setLoadSavePolicy(FIXED,SYNC);
00610 }
00611
00612
00613
00614
00615 void getFullT(fmat::Transform& t, const KinematicJoint* endj) const {
00616 if(parent!=endj) {
00617 parent->getFullT(t,endj);
00618 t*=Tq;
00619 } else {
00620 t=Tq;
00621 }
00622 }
00623 void getQuaternion(fmat::Quaternion& quat, const KinematicJoint* endj) const {
00624 if(parent!=endj) {
00625 parent->getQuaternion(quat,endj);
00626 quat *= getQuaternion();
00627 } else {
00628 quat = getQuaternion();
00629 }
00630 }
00631
00632
00633 void doSaveXMLNode(std::set<std::string>& seen, xmlNode* node, const std::string& key, const std::string& indentStr, size_t longestKeyLen) const {
00634 const_iterator it=findEntry(key);
00635 if(it==end())
00636 return;
00637 saveXMLNode(node, key, it->second, indentStr, longestKeyLen);
00638
00639 }
00640 virtual void plistValueChanged(const plist::PrimitiveBase& pl);
00641 virtual void addSelfListener();
00642 virtual void removeSelfListener();
00643 void updateTo();
00644 void updateTq();
00645 virtual void updateBB() const;
00646 void updateDepth() {
00647 if(parent==NULL)
00648 depth=0;
00649 else
00650 depth = parent->depth+1;
00651 std::for_each(branches.begin(), branches.end(), std::mem_fun(&KinematicJoint::updateDepth));
00652 }
00653
00654
00655 std::set<KinematicJoint*> branches;
00656
00657 void fireBranchAdded(KinematicJoint& val);
00658 void fireBranchRemoved(KinematicJoint& val);
00659 void fireReconfigured();
00660 mutable std::set<BranchListener*>* branchListeners;
00661
00662 size_t depth;
00663
00664 fmat::fmatReal q;
00665 fmat::Transform To;
00666 fmat::Transform Tq;
00667 mutable IKSolver * ik;
00668
00669 void setParent(LinkComponent& link) { link.parent = this; dirtyBB(); }
00670 void unsetParent(LinkComponent& link) { link.parent = NULL; dirtyBB(); }
00671
00672 class ComponentsListener : protected plist::CollectionListener {
00673 public:
00674 ComponentsListener(const plist::ArrayOf<LinkComponent>& source, KinematicJoint& kj) : plist::CollectionListener(), comps(source), parent(kj) {
00675 comps.addCollectionListener(this);
00676 }
00677 ~ComponentsListener() { comps.removeCollectionListener(this); }
00678 protected:
00679 virtual void plistCollectionEntryAdded(plist::Collection& , ObjectBase& primitive) { parent.setParent(dynamic_cast<LinkComponent&>(primitive)); }
00680 virtual void plistCollectionEntryRemoved(plist::Collection& , ObjectBase& primitive) { parent.unsetParent(dynamic_cast<LinkComponent&>(primitive)); }
00681 virtual void plistCollectionEntriesChanged(plist::Collection& ) {
00682 for(component_iterator it = comps.begin(); it!=comps.end(); ++it)
00683 parent.setParent(**it);
00684 }
00685 const plist::ArrayOf<LinkComponent>& comps;
00686 KinematicJoint& parent;
00687 } componentsListener;
00688
00689 virtual void dirtyBB() { bbDirty=true; }
00690 };
00691
00692
00693 class KinematicJointLoader : public plist::ArrayOf<KinematicJoint> {
00694 public:
00695
00696 explicit KinematicJointLoader(KinematicJoint& root, xmlNode* node)
00697 : plist::ArrayOf<KinematicJoint>(), parent(&root)
00698 {
00699 addEntry(root);
00700 loadXML(node);
00701 }
00702
00703 protected:
00704
00705 virtual bool loadXMLNode(size_t index, xmlNode* val, const std::string& comment);
00706
00707
00708 KinematicJoint* parent;
00709
00710 private:
00711 KinematicJointLoader(const KinematicJointLoader&);
00712 KinematicJointLoader& operator=(const KinematicJointLoader&);
00713 };
00714
00715
00716 class KinematicJointSaver : public plist::Array {
00717 public:
00718
00719 explicit KinematicJointSaver(const KinematicJoint& c, xmlNode* node=NULL) : plist::Array() {
00720 addEntry(const_cast<KinematicJoint&>(c));
00721 init(c.branches,node);
00722 }
00723
00724 explicit KinematicJointSaver(KinematicJoint& c, bool takeOwnership, xmlNode* node=NULL) : plist::Array() {
00725 if(takeOwnership)
00726 addEntry(&c);
00727 else
00728 addEntry(c);
00729 init(c.branches,node);
00730 }
00731
00732
00733
00734
00735 protected:
00736
00737 virtual void saveXML(xmlNode* node) const;
00738
00739 void init(const std::set<KinematicJoint*>& joints, xmlNode* node);
00740 };
00741
00742
00743
00744
00745
00746
00747 #endif