Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

KinematicJoint.h

Go to the documentation of this file.
00001 //-*-c++-*-
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 //! these let you build the link from a series of #model and #material settings instead of one static model/material for the whole link
00021 /*! note recursive structure... in this case, each is relative to its recursive parent instead of preceeding item in the array (as is done for KinematicJoint's serialization) */
00022 class LinkComponent : virtual public plist::Dictionary {
00023   friend class KinematicJoint;
00024   
00025 public:
00026   //! constructor
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   //! explicit copy constructor needed to set bbLowListener and bbHighListener (which aren't copied)
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   //! assignment, except for parent
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     // note no parent copy
00073     bbDirty = link.bbDirty;
00074     boundingBox = link.boundingBox;
00075     return *this;
00076   }
00077   
00078   virtual ~LinkComponent() {}
00079   
00080   plist::Primitive<float> mass; //!< the mass of the component in kilograms
00081   plist::Point centerOfMass; //!< the center of mass of this component
00082   
00083   plist::Primitive<bool> visible; //!< if false, will only be shown in the Mirage host's flying camera
00084   
00085   plist::Primitive<std::string> material; //!< mesh files specify "default" textures, but this can override with a different material
00086   
00087   plist::Primitive<std::string> model; //!< name of an OGRE mesh file; if set to "CollisionModel" will display collisionModel (and apply values from #collisionModelScale, #collisionModelRotation, and #collisionModelOffset)
00088   plist::Point modelScale; //!< scaling factor to apply to #model
00089   plist::Point modelRotation; //!< rotation to apply to #model (specifies axis component of quaternion, e.g. from axis/angle: axis * sin(angle/2) )
00090   plist::Point modelOffset; //!< a translation to apply to #model
00091   
00092   plist::Primitive<std::string> collisionModel; //!< name of an OGRE mesh file, or one of a set of primitives (Cube, Cylinder, Sphere)
00093   plist::Point collisionModelScale; //!< scaling factor to apply to #collisionModel
00094   plist::Point collisionModelRotation; //!< rotation to apply to #collisionModel (specifies axis component of quaternion, e.g. from axis/angle: axis * sin(angle/2) )
00095   plist::Point collisionModelOffset; //!< a translation to apply to #collisionModel
00096   
00097   //! Returns a 2D planner obstacle of this component's collision object, or NULL if no collision model is specified
00098   /*! This is a new allocation, caller is responsible for deallocation. */
00099   PlannerObstacle2D* getObstacle(const fmat::Transform& worldT) const;
00100   
00101   //! Inserts 2D projections of this link's collision components into @a obs, if @a recurse is set, recurses on branches
00102   void getObstacles(const fmat::Transform& worldT, HierarchicalObstacle& obs, bool recurse) const;
00103   
00104   //! Returns the axis-aligned bounding box (relative to link frame) of this component only (i.e. for KinematicJoint, ignores subcomponents)
00105   virtual BoundingBox3D getOwnAABB() const { return getAABB(); }
00106   
00107   //! Returns the axis-aligned bounding box (relative to link frame) of this component and any subcomponents
00108   const BoundingBox3D& getAABB() const { if(bbDirty) updateBB(); return boundingBox; }
00109   
00110   //! Computes the tightest-fitting 2D rectangle for this link (writing the result into @a ro).
00111   /*! @ro will be in the world coordinates, projecting the link collision model down the world's Z into its XY plane. */
00112   bool getOwnBB2D(const fmat::Transform& worldT, RectangularObstacle& ro) const;
00113     
00114     //! Computes a 3D box for this link (writing the result into @a bo).
00115   /*! @ro will be in the world coordinates */
00116   bool getOwnBB3D(const fmat::Transform& worldT, BoxObstacle& bo) const;
00117   
00118   //! Computes a 2D rectangle for this link and its subcomponents (writing the result into @a ro).
00119   /*! @ro will be in the world coordinates, projecting the link collision model down the world's Z into its XY plane.
00120    The rectangle will be aligned to the link axis (i.e. this is getAABB() projected into the world.) */
00121   bool getBB2D(const fmat::Transform& worldT, RectangularObstacle& ro) const;
00122     
00123     //! Computes a 3D box for this link and its subcomponents (writing the result into @a bo).
00124   /*! @ro will be in the world coordinates.
00125    The rectangle will be aligned to the link axis (i.e. this is getAABB() projected into the world.) */
00126   bool getBB3D(const fmat::Transform& worldT, BoxObstacle& bo) const;
00127   
00128   //! return the center of mass of this component, ONLY, as a homogenous scaled vector
00129   /*! The last element (homogeneous scale factor) is left as the total mass, so divide by this value to normalize.
00130    *  Otherwise, just access #centerOfMass directly. */
00131   fmat::Column<4> getMassVector() const { return fmat::pack(mass*centerOfMass[0],mass*centerOfMass[1],mass*centerOfMass[2],mass); }
00132   
00133   //! returns the center of mass of this link only, not including any branches; if this is actually a KinematicJoint, will include subcomponents
00134   virtual void sumLinkCenterOfMass(fmat::Column<3>& cOfM, float& totalMass) const;
00135   
00136   //! returns the unnormalized center of mass of this link only, not including any branches; if this is actually a KinematicJoint, will include subcomponents
00137   /*! The last element (homogeneous scale factor) is left as the total mass, so divide by this value to normalize. */
00138   virtual fmat::Column<4> sumLinkCenterOfMass() const { return getMassVector(); }
00139   
00140   //! returns true if #mass is greater than zero, of if this is a KinematicJoint, if any of the components have mass greater than zero
00141   virtual bool hasMass() const { return mass>0; }
00142   
00143   //! converts the model rotation and offset members into the supplied fmat::Transform
00144   void getModelTransform(fmat::Transform& tr) const;
00145   
00146   //! returns the model rotation and offset members as a fmat::Transform
00147   fmat::Transform getModelTransform() const {
00148     fmat::Transform tr; getModelTransform(tr); return tr;
00149   }
00150   
00151   //! converts the collision model rotation and offset members into the supplied fmat::Transform
00152   void getCollisionModelTransform(fmat::Transform& tr) const;
00153   
00154   //! returns the model rotation and offset members as a fmat::Transform
00155   fmat::Transform getCollisionModelTransform() const {
00156     fmat::Transform tr; getCollisionModelTransform(tr); return tr;
00157   }
00158   
00159   //! returns #parent joint, or NULL if this is the first joint, see addBranch()/removeBranch()
00160   KinematicJoint* getParent() const { return parent; }
00161   
00162   //! returns the next mobile ancestor joint in the kinematic chain
00163   KinematicJoint* getNextMobileAncestor() const;
00164   
00165   PLIST_CLONE_DEF(LinkComponent,new LinkComponent(*this));
00166   
00167 protected:
00168   KinematicJoint* parent; //!< if non-NULL, the parent joint to which this one is attached
00169   
00170   mutable bool bbDirty; //!< indicates bounding boxes need to be recomputed
00171   mutable BoundingBox3D boundingBox; //!< bounding box of this link (including subcomponents, but not child links)
00172   
00173   virtual void dirtyBB(); //!< sets #bbDirty to true to cause it to be recomputed on next getAABB() call
00174   virtual void updateBB() const; //!< recomputes #boundingBoxLow and #boundingBoxHigh based on collision model parameters
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   // listener callbacks should go last to ensure they are activated after members are initialized, and deactivated before they are destructed
00195   plist::PrimitiveCallbackMember<LinkComponent> collisionModelListener; //!< if going from empty string to non-empty string, indicates bounding box values need to be rebuilt
00196   plist::CollectionCallbackMember<LinkComponent> collisionModelScaleListener; //!< indicates bounding box values need to be rebuilt
00197   plist::CollectionCallbackMember<LinkComponent> collisionModelRotationListener; //!< indicates bounding box values need to be rebuilt
00198   plist::CollectionCallbackMember<LinkComponent> collisionModelOffsetListener; //!< indicates bounding box values need to be rebuilt
00199 };
00200 
00201 
00202 //! Manages parameters which define the position and type of motion produced by an actuator (i.e. forward kinematics)
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   //! constructor
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   //! copy constructor (deep copy, but doesn't register with parent)
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   //! deep copy via copyBranches(), replaces current branches.  This does not affect listener list (other than calling them), the parent or depth values
00240   KinematicJoint& operator=(const KinematicJoint& kj) {
00241     shallowCopy(kj);
00242     copyBranches(kj);
00243     return *this;
00244   }
00245   //! copies all parameters from @a kj, @e except listeners, depth, parent or children; see copyBranches()
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     // ** note what is NOT copied: **
00270     //branchListeners = kj.branchListeners;
00271     //parent = kj.parent;
00272     //depth = kj.depth;
00273     // copyBranches();
00274   }
00275   //! releases current children and clones those from @kj; does not copy parameters, see shallowCopy()
00276   void copyBranches(const KinematicJoint& kj) {
00277     if(&kj==this)
00278       return;
00279     // watch out for assigning a sub-link to an ancestor, make copies of all sub-branches from source, then clear current branches, then add previously created clones as new branches
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   //! destructor, recursively destroys sub-tree
00288   virtual ~KinematicJoint();
00289   
00290   //! types of joints which are supported
00291   enum JointType_t {
00292     REVOLUTE, //!< a joint which rotates around the z-axis
00293     PRISMATIC //!< a joint which slides along the z-axis
00294   };
00295   //! provides human readable names for JointType_t, for loading/saving #jointType (null terminated)
00296   static const char* jointTypeNames[3];
00297   //! the type of motion produced by this joint
00298   plist::NamedEnumeration<JointType_t> jointType;
00299   
00300   // parameters in the order they are applied to get to this joint's frame
00301   plist::Angle theta; //!< rotation about the previous joint's z-axis to point its x-axis parallel to the common normal with this joint's z-axis (aka "Modified Denavit-Hartenberg")
00302   plist::Primitive<fmat::fmatReal> d; //!< translation along the previous joint's z-axis to align its x-axis with the common normal with this joint's z-axis (aka "Modified Denavit-Hartenberg")
00303   plist::Angle alpha; //!< rotation about this joint's x-axis to make the previous joint's z-axis parallel to this one (aka "Modified Denavit-Hartenberg")
00304   plist::Primitive<fmat::fmatReal> r; //!< translation from the previous joint's z-axis along the common normal to place this joint's origin (aka "Modified Denavit-Hartenberg")
00305   plist::Angle qOffset; //!< a constant offset to the #q setting to define the physical zero-point of the joint
00306   
00307   plist::Angle qmin; //!< indicates the minimum q value which inverse kinematics may provide (if equal to #qmax, the joint is considered immobile)
00308   plist::Angle qmax; //!< indicates the maximum q value which inverse kinematics may provide (if equal to #qmin, the joint is considered immobile)
00309   
00310   //! a list of link features, for simple display or collision models with more than one primitive
00311   plist::ArrayOf<LinkComponent> components;
00312   typedef plist::ArrayOf<LinkComponent>::const_iterator component_iterator; //!< for convenience when looping over #components
00313   
00314   plist::Primitive<float> frictionForce; //!< conversion from velocity to friction force (default 0.5)
00315   plist::Point anistropicFrictionRatio; //!< direction sensitivity of friction, '1' in all directions means it is not direction sensitive
00316     
00317   plist::Primitive<std::string> ikSolver; //!< specifies the name of the inverse kinematics solver to use with this appendage
00318   
00319   plist::ArrayOf<SensorInfo> sensorInfo; //!< Stores sensor parameters for simulated input, type indicated by SensorType entry corresponding to a SensorInfo subclass
00320   
00321   //! Parameters for joint motion model, used for simulation
00322   /*! You should probably sync entries made here with entries in Planners/Dynamics/MotorController */
00323   struct ControllerInfo : virtual public plist::Dictionary {
00324     explicit ControllerInfo() : plist::Dictionary(), velocity(false), forceControl(false) { init(); } //!< constructor
00325     ControllerInfo(const ControllerInfo& ci) : plist::Dictionary(), velocity(ci.velocity), forceControl(ci.forceControl) { init(); } //!< copy constructor for cloning
00326     plist::Primitive<bool> velocity; //!< Adjusts interpretation of feedback and application of friction, false implies position control
00327     plist::Primitive<bool> 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.
00328   protected:
00329     //! performs common initialization
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; //!< Stores controller parameters for simulated input, type indicated by ControllerType entry corresponding to a ControllerInfo subclass
00336   
00337   
00338   plist::OutputSelector outputOffset; //!< name or index of the joint this maps to so we can integrate with Tekkotsu's flat arrays
00339   
00340   void getObstacles(const fmat::Transform& worldT, class HierarchicalObstacle& obs, bool recurse) const;
00341   
00342   //! Returns the axis-aligned bounding box (relative to link frame) of this component only (i.e. KinematicJoint ignores subcomponents)
00343   /*! KinematicJoint uses cache members to store the expanded subcomponent BB, so must recompute the local BB each time */
00344   virtual BoundingBox3D getOwnAABB() const { BoundingBox3D bb; computeOwnAABB(bb); return bb; }
00345   
00346   //! returns the current #q value
00347   float getQ() const { return q; }
00348   
00349   //! sets the current joint position (#q) (ignoring #qmin, #qmax) and updates the transformation matrix (if #q has changed)
00350   void setQ(float x) { if(x!=q) { q=x; updateTq(); } }
00351 
00352   //! sets the current joint position (#q) to x, clipped to [#qmin,#qmax], returning true if in range
00353   bool tryQ(float x);
00354   
00355   //! sets the joint position, and #qmin and #qmax as well to prevent inverse kinematics from using it
00356   void freezeQ(float x) { qmin=qmax=x; setQ(x); }
00357   
00358   //! returns true if x is within [#qmin,#qmax] (inclusive)
00359   bool validQ(float x) const { return (x>=qmin && x<=qmax); }
00360   
00361   //! sets the joint position and its childrens' positions from a flat array, using #outputOffset
00362   /*! @a deoffset will be subtracted from each index, and the result only added to the map if it (as unsigned int) is less than @a max.
00363    *  This is intended to allow you to extract the joint values for only a region of outputs in use, e.g. values[NumArmJoints], offset=ArmOffset and max=NumArmJoints */
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   //! sets the joint position and its ancestors' positions from a flat array, using #outputOffset
00372   /*! @a deoffset will be subtracted from each index, and the result only added to the map if it (as unsigned int) is less than @a max.
00373    *  This is intended to allow you to extract the joint values for only a region of outputs in use, e.g. values[NumArmJoints], offset=ArmOffset and max=NumArmJoints */
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   //! sets the joint position and its childrens' positions from a flat array, using #outputOffset
00382   /*! @a deoffset will be subtracted from each index, and the result only added to the map if it (as unsigned int) is less than @a max.
00383    *  This is intended to allow you to extract the joint values for only a region of outputs in use, e.g. values[NumArmJoints], offset=ArmOffset and max=NumArmJoints */
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   //! sets the joint position and its ancestors' positions from a flat array, using #outputOffset
00392   /*! @a deoffset will be subtracted from each index, and the result only added to the map if it (as unsigned int) is less than @a max.
00393    *  This is intended to allow you to extract the joint values for only a region of outputs in use, e.g. values[NumArmJoints], offset=ArmOffset and max=NumArmJoints */
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   //! sets the joint position and its childrens' to zero
00402   void zeroChildrenQ();
00403   
00404   //! sets the joint position and its ancestors' to zero
00405   void zeroAncestorQ();
00406   
00407   
00408   //! returns the tranformation matrix which converts from the link's reference frame (i.e. the frame which moves with the joint's #q) to the base frame
00409   fmat::Transform getFullT() const;
00410   
00411   //! returns the tranformation matrix which converts from the base frame to the link's reference frame (i.e. the frame which moves with the joint's #q)
00412   fmat::Transform getFullInvT() const { return getFullT().rigidInverse(); }
00413   
00414   //! returns the tranformation matrix which converts from the link's reference frame (i.e. the frame which moves with the joint's #q) to the specified joint's link frame
00415   fmat::Transform getT(const KinematicJoint& j) const;
00416   
00417   //! returns the tranformation matrix which converts from the joint's reference frame (i.e. the frame in which the link moves) to the parent's frame
00418   /*! This transformation is constant in terms of #q. */
00419   const fmat::Transform& getTo() const { return To; }
00420   
00421   //! returns the tranformation matrix which converts from the link's reference frame (i.e. the frame which moves with the joint's #q) to the parent's frame
00422   /*! This transformation is changes with #q. */
00423   const fmat::Transform& getTq() const { return Tq; }
00424   
00425   //! returns the center of mass of this link and all of its branches, given their current positions, resulting position relative to this link
00426   void sumCenterOfMass(fmat::Column<3>& cOfM, float& totalMass) const;
00427   
00428   //! returns the unnormalized center of mass of this link and all of its branches, given their current positions, relative to this link
00429   /*! The last element (homogeneous scale factor) is left as the total mass, so divide by this value to normalize. */
00430   fmat::Column<4> sumCenterOfMass() const;
00431   
00432   //! returns the unnormalized center of mass of this link only, not including any branches
00433   /*! The last element (homogeneous scale factor) is left as the total mass, so divide by this value to normalize. */
00434   virtual fmat::Column<4> sumLinkCenterOfMass() const;
00435   
00436   using LinkComponent::sumLinkCenterOfMass;
00437   
00438   virtual bool hasMass() const;
00439   
00440   //! returns a column vector indicating the joint's motion in base coordinates, at a point @a p (in base coordinates)
00441   /*! The first 3 rows indicate the ∂x/∂q, ∂y/∂q, and ∂z/∂q values.
00442    *  The last 3 rows indicate the axis of rotation (or all 0's for a prismatic joint) */
00443   fmat::Column<6> getJointJacobian(const fmat::Column<3>& p) const;
00444   
00445   //! computes the motion of this joint and all of its ancestors at a point @a p (in base coordinates), @a j should be empty on call, may wish to reserve #depth+1 
00446   /*! The first 3 rows indicate the ∂x, ∂y, and ∂z values per ∂q corresponding to each column.
00447    *  The last 3 rows indicate the axis of rotation for each joint (or all 0's for prismatic joints) */
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   //! computes the motion of this joint and all of its ancestors at a point @a p (in base coordinates), @a j should be empty on call, may wish to reserve #depth+1 
00455   /*! The first 3 rows indicate the ∂x, ∂y, and ∂z values per ∂q corresponding to each column.
00456    *  The last 3 rows indicate the axis of rotation for each joint (or all 0's for prismatic joints) */
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   //! returns the position of this joint in base coordinates (as a length 3 vector)
00465   fmat::Column<3> getWorldPosition() const { return getFullT().translation(); }
00466   //! returns the orientation of this joint in base coordinates (as a 3x3 matrix)
00467   fmat::Matrix<3,3> getWorldRotation() const { return getFullT().rotation(); }
00468   
00469   //! returns the position of this joint relative to its parent's link frame
00470   fmat::Column<3> getPosition() const { return Tq.translation(); }
00471   //! returns the position of the parent origin relative to this joint's link frame
00472   fmat::Column<3> getParentPosition() const;
00473   //! returns the orientation of this joint relative to its parent's link frame (as a 3x3 matrix)
00474   fmat::Matrix<3,3> getRotation() const { return Tq.rotation(); }
00475   
00476   //! returns the orientation of this joint in base coordinates (as a quaternion (w,x,y,z))
00477   fmat::Quaternion getWorldQuaternion() const;
00478   //! returns the orientation of this joint in base coordinates (as a quaternion (w,x,y,z))
00479   fmat::Quaternion getQuaternion(const KinematicJoint& j) const;
00480   //! returns the orientation of this joint relative to its parent's link frame (as a quaternion (w,x,y,z))
00481   fmat::Quaternion getQuaternion() const { return fmat::Quaternion::fromMatrix(Tq.rotation()); }
00482   
00483   //! if the node is an array, loads the sub-tree, otherwise expects a dict to load the joint's own parameters
00484   virtual void loadXML(xmlNode* node);
00485   //! if the node is NULL, has no parent, or is a plist node, writes human-readable help text as a comment and then recurses through the subtrees (otherwise saves only this joint's parameters)
00486   virtual void saveXML(xmlNode* node, bool onlyOverwrite, std::set<std::string>& seen) const;
00487   using plist::Dictionary::saveXML;
00488   
00489   
00490   //! Builds a map from output offsets to this and child joints (call it on the root node –– doesn't handle ancestors
00491   /*! The map must be initialized to NULL values before calling this method.
00492    * @a deoffset will be subtracted from each index, and the result only added to the map if it (as unsigned int) is less than @a max.
00493    *  This is intended to allow you to extract the mapping for only a region of outputs in use, e.g. childMap[NumArmJoints], offset=ArmOffset and max=NumArmJoints */
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   //! builds a map from output offsets to this and child joints (call it on the root node –– doesn't handle ancestors
00503   /*! The map must be initialized to NULL values before calling this method.
00504    * @a deoffset will be subtracted from each index, and the result only added to the map if it (as unsigned int) is less than @a max.
00505    *  This is intended to allow you to extract the mapping for only a region of outputs in use, e.g. childMap[NumArmJoints], offset=ArmOffset and max=NumArmJoints */
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   //! builds a map from output offsets to this and child joints (call it on the root node –– doesn't handle ancestors
00515   /*! The map must be initialized to NULL values before calling this method.
00516    * @a deoffset will be subtracted from each index, and the result only added to the map if it (as unsigned int) is less than @a max.
00517    *  This is intended to allow you to extract the mapping for only a region of outputs in use, e.g. childMap[NumArmJoints], offset=ArmOffset and max=NumArmJoints */
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   //! builds a map from output offsets to this and child joints (call it on the root node –– doesn't handle ancestors
00526   /*! The map must be initialized to NULL values before calling this method.
00527    * @a deoffset will be subtracted from each index, and the result only added to the map if it (as unsigned int) is less than @a max.
00528    *  This is intended to allow you to extract the mapping for only a region of outputs in use, e.g. childMap[NumArmJoints], offset=ArmOffset and max=NumArmJoints */
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   //! type of iterators over sub-trees
00537   typedef std::set<KinematicJoint*>::const_iterator branch_iterator;
00538   
00539   //! returns a reference to #branches so you can iterator over sub-trees
00540   const std::set<KinematicJoint*>& getBranches() const { return branches; }
00541   
00542   //! adds the joint to the #branches, control over memory (de)allocation is assumed by this instance until/unless removeBranch() is called
00543   /*! For convenience, returns @a b */
00544   KinematicJoint* addBranch(KinematicJoint* b);
00545   
00546   //! removes the specified joint from #branches, returning control of (de)allocation of @a b to caller.
00547   /*! For convenience, returns @a b */
00548   KinematicJoint* removeBranch(KinematicJoint* b);
00549   
00550   //! deletes all entries of #branches en masse
00551   void clearBranches();
00552   
00553   //! returns true if the link following this joint has more than one additional joint attached (i.e. if #branches.size() > 1)
00554   bool isBranch() const { return branches.size()>1; }
00555   
00556   //! Clones joints from this back to and including the root, not cloning side branches; returns a pointer to the @e last (i.e. leaf) joint in the cloned branch.
00557   /*! To clone a joint and all its children (instead of its direct ancestors as is done here), use the copy constructor or operator=.
00558     *  Just use getRoot() on the result if you want to store the root joint instead of the end joint. */
00559   KinematicJoint* cloneBranch() const;
00560   
00561   //! returns the 'first' element of #branches, or NULL if this is a leaf
00562   /*! The choice of 'first' is arbitrary (and not necessarily consistent run-to-run), you should not use this if you expect to encounter branching chains */
00563   KinematicJoint* nextJoint() const { return *branches.begin(); }
00564   
00565   //! returns true if qmin≠qmax (i.e. the joint is considered "mobile", and not just a abstract reference frame)
00566   bool isMobile() const { return qmin!=qmax; }
00567   
00568   //! returns root joint (will return 'this' if this is the first joint), see addBranch()/removeBranch()
00569   KinematicJoint* getRoot() { KinematicJoint* kj=this; while(kj->parent!=NULL) kj=kj->parent; return kj; }
00570   //! returns root joint (will return 'this' if this is the first joint), see addBranch()/removeBranch()
00571   const KinematicJoint* getRoot() const { const KinematicJoint* kj=this; while(kj->parent!=NULL) kj=kj->parent; return kj; }
00572   //! returns a string containing the names of all ancestors' outputOffset values separated by @a sep
00573   std::string getPath(const std::string& sep = "/") const { return (parent==NULL ? std::string() : parent->getPath(sep)) + sep + outputOffset.get(); }
00574   //! returns 0 if this is the root (no parent), otherwise one plus the parent's depth
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   //! returns an IKSolver instance, which will be cached internally for future accesses
00582   IKSolver& getIK() const;
00583   
00584   PLIST_CLONE_DEF(KinematicJoint,new KinematicJoint(*this));
00585   
00586 protected:
00587   //! add the plist entries to the dictionary superclass
00588   void initEntries() {
00589     // Since we're going to have a lot of KinematicJoints listed together, we're not going to add
00590     // a description comment here, which would be repeated for each instance.
00591     // Instead, a single comment will be dumped at the beginning of the array of joints.
00592     addEntry("JointType",jointType);
00593     addEntry("θ",theta); // aka \u03B8, but we use the character literal because we explicitly want UTF-8 in the file output
00594     addEntry("d",d);
00595     addEntry("α",alpha); // aka \u03B1, but we use the character literal because we explicitly want UTF-8 in the file output
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); // accept anything (i.e. reference frames are fine)
00608     addSelfListener();
00609     setLoadSavePolicy(FIXED,SYNC);
00610   }
00611   
00612   //! recursively computes the full transformation matrix, converting from this joint's frame to the specified joint's frame, or the base frame if @end is NULL
00613   /*! @a t does not need to be initialized to anything prior to call, but will be 4x4 on return \n
00614     *  @a endj @e must be an ancestor of this joint or the function will segfault (NULL is the ancestor of the root, so that's valid) */
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   //! shorthand for saving a specific node, forwarding call to saveXMLNode()
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     //seen.insert(key);
00639   }
00640   virtual void plistValueChanged(const plist::PrimitiveBase& pl);
00641   virtual void addSelfListener(); //!< subscribes the instance to be notified of changes to its public plist::Primitive members, and then calls updateTo()
00642   virtual void removeSelfListener(); //!< unsubscribes the instance from its public plist::Primitive members
00643   void updateTo(); //!< regenerates #To from the a, d, alpha, and theta parameters, includes call to updateTq() as well
00644   void updateTq(); //!< updates #Tq from the q and qOffset parameters (based on current #To)
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   //! sub-chains of joints, supporting branches but not cycles (nor NULL entries)
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; //!< one plus #parent's depth, or 0 if parentless
00663   
00664   fmat::fmatReal q; //!< current joint position (radian rotation about z if revolute, displacement along z if prismatic)
00665   fmat::Transform To; //!< transformation to the joint's origin
00666   fmat::Transform Tq; //!< transformation to origin, including final q rotation
00667   mutable IKSolver * ik; //!< an instance of the IKSolver corresponding to #ikSolver
00668 
00669   void setParent(LinkComponent& link) { link.parent = this; dirtyBB(); } //!< for use by ComponentsListener, work around #parent being protected
00670   void unsetParent(LinkComponent& link) { link.parent = NULL; dirtyBB(); } //!< for use by ComponentsListener, work around #parent being protected
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& /*col*/, ObjectBase& primitive) { parent.setParent(dynamic_cast<LinkComponent&>(primitive)); }
00680     virtual void plistCollectionEntryRemoved(plist::Collection& /*col*/, ObjectBase& primitive) { parent.unsetParent(dynamic_cast<LinkComponent&>(primitive)); }
00681     virtual void plistCollectionEntriesChanged(plist::Collection& /*col*/) {
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 //! handles the recursive loading of a tree of joints
00693 class KinematicJointLoader : public plist::ArrayOf<KinematicJoint> {
00694 public:
00695   //! constructor, start loading
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   //! for each node, if it's an array, start loading the subtree via recursive instantiation, otherwise load the KinematicJoint
00705   virtual bool loadXMLNode(size_t index, xmlNode* val, const std::string& comment);
00706   
00707   //! the current joint for which sub-joints are being loaded
00708   KinematicJoint* parent;
00709   
00710 private:
00711   KinematicJointLoader(const KinematicJointLoader&); //!< don't call
00712   KinematicJointLoader& operator=(const KinematicJointLoader&); //!< don't call
00713 };
00714 
00715 //! handles the recursive saving of a tree of joints
00716 class KinematicJointSaver : public plist::Array {
00717 public:
00718   //! given a single KinematicJoint, places it at the beginning of the array, followed by its children
00719   explicit KinematicJointSaver(const KinematicJoint& c, xmlNode* node=NULL) : plist::Array() {
00720     addEntry(const_cast<KinematicJoint&>(c));
00721     init(c.branches,node);
00722   }
00723   //! given a single KinematicJoint, places it at the beginning of the array, followed by its children
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   /* given a set of joints, adds the chain as elements of the array, so that they can be saved in order (by a second call from client)
00732   explicit KinematicJointSaver(const std::set<KinematicJoint*>& joints, xmlNode* node=NULL) : plist::Array() {
00733     init(joints,node);
00734   }*/
00735 protected:
00736   //! saves the array of joints, prepending human-readable help text as a comment if this is a root XML node
00737   virtual void saveXML(xmlNode* node) const;
00738   //! adds the chain of joints as elements in the array, recursing on branching nodes to insert sub-arrays
00739   void init(const std::set<KinematicJoint*>& joints, xmlNode* node);
00740 };
00741 
00742 /*! @file
00743  * @brief Describes KinematicJoint, which manages parameters defining the position and type of motion produced by an actuator (i.e. forward kinematics)
00744  * @author Ethan Tira-Thompson (ejt) (Creator)
00745  */
00746 
00747 #endif

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:42 2016 by Doxygen 1.6.3