/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/include/kinematicChain.h
Go to the documentation of this file.00001 //###################################################################### 00002 // 00003 // GraspIt! 00004 // Copyright (C) 2002-2009 Columbia University in the City of New York. 00005 // All rights reserved. 00006 // 00007 // GraspIt! is free software: you can redistribute it and/or modify 00008 // it under the terms of the GNU General Public License as published by 00009 // the Free Software Foundation, either version 3 of the License, or 00010 // (at your option) any later version. 00011 // 00012 // GraspIt! is distributed in the hope that it will be useful, 00013 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00014 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00015 // GNU General Public License for more details. 00016 // 00017 // You should have received a copy of the GNU General Public License 00018 // along with GraspIt!. If not, see <http://www.gnu.org/licenses/>. 00019 // 00020 // Author(s): Andrew T. Miller and Matei T. Ciocarlie 00021 // 00022 // $Id: kinematicChain.h,v 1.20 2009/06/17 21:10:38 saint Exp $ 00023 // 00024 //###################################################################### 00025 00026 #ifndef _kinematicchain_h_ 00027 #define _kinematicchain_h_ 00028 00029 #include <QTextStream> 00030 #include <vector> 00031 00032 #include "collisionStructures.h" 00033 #include "matvec3D.h" 00034 00035 class SoSeparator; 00036 class SoTransform; 00037 00038 class Body; 00039 class Link; 00040 class Robot; 00041 class Joint; 00042 class DynJoint; 00043 class Contact; 00044 class Matrix; 00045 class TiXmlElement; 00046 00048 00061 class KinematicChain { 00063 Robot *owner; 00064 00066 int chainNum; 00067 00069 00071 int firstJointNum; 00072 00074 int numDOF; 00075 00077 int numJoints; 00078 00080 int numLinks; 00081 00083 std::vector<Joint *> jointVec; 00084 00086 std::vector<Link *> linkVec; 00087 00089 00091 int *lastJoint; 00092 00094 transf tran; 00095 00097 transf endTran; 00098 00100 SoSeparator *IVRoot; 00101 00103 SoTransform *IVTran; 00104 00106 bool jointsMoved; 00107 00109 std::vector<Robot *> children; 00110 00112 std::vector<transf> childOffsetTran; 00113 00115 int numChildren; 00116 00118 int createDynamicJoints(const std::vector<int> &dynJointTypes); 00119 00121 void getDynamicJoints(std::vector<DynJoint*> *dj) const; 00122 00123 public: 00127 KinematicChain(Robot *r,int chainNumber, int jointNum); 00128 00129 ~KinematicChain(); 00130 00132 Matrix linkJacobian(bool worldCoords) const; 00134 Matrix activeLinkJacobian(bool worldCoords); 00136 Matrix actuatedJacobian(const Matrix &fullColumnJ) const; 00137 00138 Matrix jointTorquesVector(Matrix fullRobotTorques); 00139 00141 int getNumContacts(Body *body); 00142 00144 std::list<Contact*> getContacts(Body *body); 00145 00148 int initChainFromXml(const TiXmlElement* root,QString &linkDir); 00149 00151 void cloneFrom(const KinematicChain *original); 00152 00154 void setJointValues(const double *jointVals); 00156 void getJointValues(double *jointVals) const; 00158 void fwdKinematics(const double *jointVals, std::vector<transf> &newLinkTranVec) const; 00160 void getJointLocations(const double *jointVals, std::vector<transf> &jointTranVec) const; 00162 void infinitesimalMotion(const double *jointVals, std::vector<transf> &newLinkTranVec) const; 00164 void updateLinkPoses(); 00166 void filterCollisionReport(CollisionReport &colReport); 00168 int getFirstJointNum(){return firstJointNum;} 00169 00171 int getNum() const {return chainNum;} 00172 00173 void attachRobot(Robot *r,const transf &offsetTr); 00174 void detachRobot(Robot *r); 00175 00177 const Robot* getOwner() const {return owner;} 00179 int getNumJoints() const {return numJoints;} 00181 int getNumLinks() const {return numLinks;} 00183 Joint *getJoint(int i) const {return jointVec[i];} 00185 std::list<Joint*> getJoints(); 00187 Link *getLink(int i) const {return linkVec[i];} 00189 int getLastJoint(int i) const {return lastJoint[i];} 00191 SoSeparator *getIVRoot() const {return IVRoot;} 00193 SoTransform *getIVTran() const {return IVTran;} 00195 transf const &getTran() const {return tran;} 00198 bool jointsHaveMoved() const {return jointsMoved;} 00200 int getNumAttachedRobots() const {return numChildren;} 00202 Robot *getAttachedRobot(int i) const {return children[i];} 00203 00206 transf const &getAttachedRobotOffset(int i) const{return childOffsetTran[i];} 00207 }; 00208 00209 #endif