A serial chain of links connected by joints. More...
#include <kinematicChain.h>
Public Member Functions | |
| Matrix | activeLinkJacobian (bool worldCoords) |
| Builds the link jacobian but removes the rows corresponding to links that have no contacts. | |
| Matrix | actuatedJacobian (const Matrix &fullColumnJ) const |
| Returns a matrix that only contains the columns of the Jacobian that correspond to actuated joint dofs. | |
| void | attachRobot (Robot *r, const transf &offsetTr) |
| void | cloneFrom (const KinematicChain *original) |
| void | detachRobot (Robot *r) |
| void | filterCollisionReport (CollisionReport &colReport) |
| Keep in the report only those collisions which involve at least a link from this chain. | |
| void | fwdKinematics (const double *jointVals, std::vector< transf > &newLinkTranVec) const |
| Compute the link transforms that correspond to a given set of joint values. | |
| Robot * | getAttachedRobot (int i) const |
| transf const & | getAttachedRobotOffset (int i) const |
| std::list< Contact * > | getContacts (Body *body) |
| Returns all the contacts between links of this chain and a given object. | |
| int | getFirstJointNum () |
| Returns the number of the first joint in this chain in the robot numbering scheme. | |
| SoSeparator * | getIVRoot () const |
| SoTransform * | getIVTran () const |
| Joint * | getJoint (int i) const |
| void | getJointLocations (const double *jointVals, std::vector< transf > &jointTranVec) const |
| Compute the locations and orientations of all the joints in this chain for some set of joint values. | |
| std::list< Joint * > | getJoints () |
| Returns a list of all joints in this chain. | |
| void | getJointValues (double *jointVals) const |
| Gets joint values in a vector with values for all robot joints. | |
| int | getLastJoint (int i) const |
| Link * | getLink (int i) const |
| int | getNum () const |
| Returns the index of this chain in the robot's list of chains. | |
| int | getNumAttachedRobots () const |
| int | getNumContacts (Body *body) |
| Returns the number of contacts between links of this chain and a given object. | |
| int | getNumJoints () const |
| int | getNumLinks () const |
| const Robot * | getOwner () const |
| transf const & | getTran () const |
| void | infinitesimalMotion (const double *jointVals, std::vector< transf > &newLinkTranVec) const |
| Compute the link transforms for an infinitesimal joint motion in specified direction. | |
| int | initChainFromXml (const TiXmlElement *root, QString &linkDir) |
| bool | jointsHaveMoved () const |
| Matrix | jointTorquesVector (Matrix fullRobotTorques) |
| KinematicChain (Robot *r, int chainNumber, int jointNum) | |
| Matrix | linkJacobian (bool worldCoords) const |
| Builds the complete link jacobian wrt to dynamic joints of this chain. | |
| void | setJointValues (const double *jointVals) |
| Sets joint values (without updating poses) based on a vector with values for all robot joints. | |
| void | updateLinkPoses () |
| Set the transforms of the links to match the current joint values. | |
| ~KinematicChain () | |
Private Member Functions | |
| int | createDynamicJoints (const std::vector< int > &dynJointTypes) |
| Creates the dynamic joints for each link, based on the regular joints which must already exist. | |
| void | getDynamicJoints (std::vector< DynJoint * > *dj) const |
| Creates a list of the dynamic joints that make up this chain. | |
Private Attributes | |
| int | chainNum |
| Indicates which chain this is within the robot definition. | |
| std::vector< transf > | childOffsetTran |
| The offset transforms relating the base frame of each child robot to the end transform of the chain. | |
| std::vector< Robot * > | children |
| A vector of child robots connected to the last link of the chain. | |
| transf | endTran |
| The current end transform of the chain relative to the robot base. | |
| int | firstJointNum |
| The number of the first joint in this chain in the robot's list of joints. | |
| SoSeparator * | IVRoot |
| A pointer to the root of the chain's Inventor scene graph. | |
| SoTransform * | IVTran |
| A pointer to the Inventor transform corresponding to the base transform of the chain. | |
| bool | jointsMoved |
| Indicates whether any of the joints in the chain have moved since the last update of the kinematics. | |
| std::vector< Joint * > | jointVec |
| A vector of pointers to the joints in the chain. | |
| int * | lastJoint |
| For any link, gives us the number of the last joint in the chain that affects its pose. | |
| std::vector< Link * > | linkVec |
| A vector of pointers to the links in the chain. | |
| int | numChildren |
| The number of child robots connected to the chain. | |
| int | numDOF |
| The number of degrees of freedom in the chain. | |
| int | numJoints |
| The number of joints in the chain. | |
| int | numLinks |
| The number of links in the chain. | |
| Robot * | owner |
| Pointer to the robot that this chain is a part of. | |
| transf | tran |
| The base transform of the chain relative to the robot base. | |
A serial chain of links connected by joints.
A kinematicChain is a serial chain of links connected by joints. In a robot hand each finger is a kinematic chain. A chain can also have any number of robots attached to its end. For example, a robot hand can be connected to the end of an arm's kinematic chain. Each chain has a base transform that relates the start of the chain to the base frame of the robot. Given values for the joint positions, this class solves the forward kinematics for each link in the chain.
In general, the only way that robot links should ever be moved is through the kinematic chains: the robot asks the dof's for joint values, tells the chains to set those values then tells the chains to update link poses.
Definition at line 61 of file kinematicChain.h.
| KinematicChain::KinematicChain | ( | Robot * | r, | |
| int | chainNumber, | |||
| int | jointNum | |||
| ) |
The constructor is called from a robot's load method, and requires a a pointer to the robot owning this chain, and the index of which chain this is within the robot.
Definition at line 52 of file kinematicChain.cpp.
| KinematicChain::~KinematicChain | ( | ) |
The destructor deletes each of the joints in this chain, and asks the world to delete each of the links in the chain. It also detaches any connected robots.
Definition at line 62 of file kinematicChain.cpp.
| Matrix KinematicChain::activeLinkJacobian | ( | bool | worldCoords | ) |
Builds the link jacobian but removes the rows corresponding to links that have no contacts.
The active link jacobian will build the regular link Jacobian, then only keep the rows that correspond to links that have at least one contact (as links with no contacts can not balance actuation forces applied to them).
Definition at line 647 of file kinematicChain.cpp.
Returns a matrix that only contains the columns of the Jacobian that correspond to actuated joint dofs.
This function takes a Jacobian with all columns (6dof per joint and returns a version that has only those columns that correspond to the actuated dofs in each joint (e.g. the z axis for revolute joints)
Definition at line 679 of file kinematicChain.cpp.
Given a pointer to another robot and an offset transform of the base frame of the robot with respect to the end transform of the link of this chain.
Definition at line 379 of file kinematicChain.cpp.
| void KinematicChain::cloneFrom | ( | const KinematicChain * | original | ) |
Creates an indentical copy of another chain, sharing things such as geometry
Copies this chain structure from an existing chain. All joints are set to independent copies of the original chain. All links are created as clones of the links from the original chain.
Definition at line 333 of file kinematicChain.cpp.
| int KinematicChain::createDynamicJoints | ( | const std::vector< int > & | dynJointTypes | ) | [private] |
Creates the dynamic joints for each link, based on the regular joints which must already exist.
Creates dynamic joints for each of the links in the chain. A dynamic joint can be a collection of one or more regular joints, so dynamic joints are only created when all the links and the regular joints are already in place. The vector of dynamic joint types tells us what kind of dynamic joint each link is connected to, and the dynamic joint is then constructed based on the appropriate number of regular joints.
See the DynJoint class for details.
Definition at line 92 of file kinematicChain.cpp.
| void KinematicChain::detachRobot | ( | Robot * | r | ) |
This separates the robot pointed to by r from this kinematic chain, allowing them to move independently.
Definition at line 398 of file kinematicChain.cpp.
| void KinematicChain::filterCollisionReport | ( | CollisionReport & | colReport | ) |
Keep in the report only those collisions which involve at least a link from this chain.
Filters a collision report and keeps only collisions of a particular chain For now it also throws out collisions between robot parts, as those are giving us problems.
Definition at line 426 of file kinematicChain.cpp.
| void KinematicChain::fwdKinematics | ( | const double * | jointVals, | |
| std::vector< transf > & | newLinkTranVec | |||
| ) | const |
Compute the link transforms that correspond to a given set of joint values.
Given a array of joint values for each joint in the chain, this method will compute the transforms for each link in the chain with respect to world coordinates. It does not affect the chain itself.
The array of joint values is assumed to contain all the joints in the robot, numbered as in the robot's numbering scheme. Alternatively, you can pass NULL instead, in which case the current joint values are used.
Definition at line 522 of file kinematicChain.cpp.
| Robot* KinematicChain::getAttachedRobot | ( | int | i | ) | const [inline] |
Returns the i-th robot attached to the end of the chain
Definition at line 202 of file kinematicChain.h.
| transf const& KinematicChain::getAttachedRobotOffset | ( | int | i | ) | const [inline] |
Returns the offset transform between the end of the chain and the base of the i-th attached robot
Definition at line 206 of file kinematicChain.h.
Returns all the contacts between links of this chain and a given object.
Returns a list of the contacts between the links of this chain and the object body. If body == NULL, returns all the contacts, regardless of what object they are against. Todo: what about self-colision?
Definition at line 748 of file kinematicChain.cpp.
| void KinematicChain::getDynamicJoints | ( | std::vector< DynJoint * > * | dj | ) | const [private] |
Creates a list of the dynamic joints that make up this chain.
Definition at line 611 of file kinematicChain.cpp.
| int KinematicChain::getFirstJointNum | ( | ) | [inline] |
Returns the number of the first joint in this chain in the robot numbering scheme.
Definition at line 168 of file kinematicChain.h.
| SoSeparator* KinematicChain::getIVRoot | ( | ) | const [inline] |
Returns a pointer to the root of the chain's Inventor scene graph
Definition at line 191 of file kinematicChain.h.
| SoTransform* KinematicChain::getIVTran | ( | ) | const [inline] |
Returns a pointer to the Inventor transform corresponding to the base transform of the chain
Definition at line 193 of file kinematicChain.h.
| Joint* KinematicChain::getJoint | ( | int | i | ) | const [inline] |
Returns a pointer to the i-th joint in the chain
Definition at line 183 of file kinematicChain.h.
| void KinematicChain::getJointLocations | ( | const double * | jointVals, | |
| std::vector< transf > & | jointTranVec | |||
| ) | const |
Compute the locations and orientations of all the joints in this chain for some set of joint values.
Given an array of joint values for each joint in the chain, it will compute the location and orientations of all the joints in the chain, w.r.t. world coordinates. This is somewhat similar to fwd kinematics, but instead of computing link transforms it computes joint transforms. This is different for two reasons. First, a joint's coordinate system is the chained transform before* that joint, not after the joint (as it would be for a link that comes after the joint). Also, we can have multiple joints between two links.
Also remember two GraspIt conventions: for any joint, the joint axis (for rotation or translation) is the z axis of the joint transform returned here. Also, the origin of a link is the same as the transform *after* the last joint that comes before it.
The array of joint values is assumed to contain all the joints in the robot, numbered as in the robot's numbering scheme. Alternatively, you can pass NULL instead, in which case the current joint values are used.
Definition at line 558 of file kinematicChain.cpp.
| std::list< Joint * > KinematicChain::getJoints | ( | ) |
Returns a list of all joints in this chain.
Definition at line 414 of file kinematicChain.cpp.
| void KinematicChain::getJointValues | ( | double * | jointVals | ) | const |
Gets joint values in a vector with values for all robot joints.
Reads in the current values of the joints in this chain, and uses them to populate the vector jointVals. This vector is ordered to contain all the joints of the robot (not only of this chain) and joints are indexed by their number in the robot structure, not in this chain's struture.
Definition at line 465 of file kinematicChain.cpp.
| int KinematicChain::getLastJoint | ( | int | i | ) | const [inline] |
Returns the index of the last joint in the chain that affects the pose of link i
Definition at line 189 of file kinematicChain.h.
| Link* KinematicChain::getLink | ( | int | i | ) | const [inline] |
Returns a pointer to the i-th link in the chain
Definition at line 187 of file kinematicChain.h.
| int KinematicChain::getNum | ( | ) | const [inline] |
Returns the index of this chain in the robot's list of chains.
Definition at line 171 of file kinematicChain.h.
| int KinematicChain::getNumAttachedRobots | ( | ) | const [inline] |
Returns the number of robots attached to the end of the chain
Definition at line 200 of file kinematicChain.h.
| int KinematicChain::getNumContacts | ( | Body * | body | ) |
Returns the number of contacts between links of this chain and a given object.
Returns the number of contacts between the links of this chain and the object body. If body == NULL, returns the total number of contacts, regardless of what object they are against. Todo: what about self-colision?
Definition at line 733 of file kinematicChain.cpp.
| int KinematicChain::getNumJoints | ( | ) | const [inline] |
Returns the number of joints in the chain
Definition at line 179 of file kinematicChain.h.
| int KinematicChain::getNumLinks | ( | ) | const [inline] |
Returns the number of links in the chain
Definition at line 181 of file kinematicChain.h.
| const Robot* KinematicChain::getOwner | ( | ) | const [inline] |
Return the robot that owns this chain
Definition at line 177 of file kinematicChain.h.
| transf const& KinematicChain::getTran | ( | ) | const [inline] |
Returns the base transform of the chain relative to the robot base
Definition at line 195 of file kinematicChain.h.
| void KinematicChain::infinitesimalMotion | ( | const double * | jointVals, | |
| std::vector< transf > & | newLinkTranVec | |||
| ) | const |
Compute the link transforms for an infinitesimal joint motion in specified direction.
Given an array of desired joint values, this computed an infinitesimal motion of each link as motion *from the current values* towards the desired values is started. Used mainly to see if any contacts prevent this motion.
Definition at line 576 of file kinematicChain.cpp.
| int KinematicChain::initChainFromXml | ( | const TiXmlElement * | root, | |
| QString & | linkDir | |||
| ) |
Loads the chain information from XML, expected in the standard format of GraspIt! .xml robot files
Sets up the chain given a XML DOM from a currently open robot configuration file. It reads the number of joints and the number of links and allocates space for those vectors, then it reads in the base transform of the chain. Next, it reads a node for each joint and creates a prismatic or revolute joint which is initialized with the kinematic data in that node. linkDir should be the path to the directory where the link body files are kept (usually rootPath/iv).
Definition at line 146 of file kinematicChain.cpp.
| bool KinematicChain::jointsHaveMoved | ( | ) | const [inline] |
Returns the value of the flag indicating whether the joints have been moved since the last time the link poses have been updated
Definition at line 198 of file kinematicChain.h.
Given a matrix with all the joint torques of the robot, numbered as in the the robot scheme, this extracts a vector of chain joint torques, in the order in which they appear in this chain
Definition at line 717 of file kinematicChain.cpp.
| Matrix KinematicChain::linkJacobian | ( | bool | worldCoords | ) | const |
Builds the complete link jacobian wrt to dynamic joints of this chain.
The link jacobian relates joint angle changes to link movement. Its transpose relates forces applied to link to forces applied to joints. If worldCoords is false, the jacobian is computed in each link's coordinate system.
Definition at line 627 of file kinematicChain.cpp.
| void KinematicChain::setJointValues | ( | const double * | jointVals | ) |
Sets joint values (without updating poses) based on a vector with values for all robot joints.
Sets the current joint values from the vector jointVals. This vector is ordered to contain all the joints of the robot (not only of this chain) and joints are indexed by their number in the robot structure, not in this chain's struture.
Definition at line 478 of file kinematicChain.cpp.
| void KinematicChain::updateLinkPoses | ( | ) |
Set the transforms of the links to match the current joint values.
Computes forward kinematics for the current joint values, then sets the link transforms accordingly. This is the only way that robot links should ever be moved (the robot asks the dof' for joint values, tells the chains to set those values then tells the chains to update link poses).
Definition at line 492 of file kinematicChain.cpp.
int KinematicChain::chainNum [private] |
Indicates which chain this is within the robot definition.
Definition at line 66 of file kinematicChain.h.
std::vector<transf> KinematicChain::childOffsetTran [private] |
The offset transforms relating the base frame of each child robot to the end transform of the chain.
Definition at line 112 of file kinematicChain.h.
std::vector<Robot *> KinematicChain::children [private] |
A vector of child robots connected to the last link of the chain.
Definition at line 109 of file kinematicChain.h.
transf KinematicChain::endTran [private] |
The current end transform of the chain relative to the robot base.
Definition at line 97 of file kinematicChain.h.
int KinematicChain::firstJointNum [private] |
The number of the first joint in this chain in the robot's list of joints.
This allows us to compute correspondances between joints in this chain's list and joints in the robot's overall list, which spans all chains.
Definition at line 71 of file kinematicChain.h.
SoSeparator* KinematicChain::IVRoot [private] |
A pointer to the root of the chain's Inventor scene graph.
Definition at line 100 of file kinematicChain.h.
SoTransform* KinematicChain::IVTran [private] |
A pointer to the Inventor transform corresponding to the base transform of the chain.
Definition at line 103 of file kinematicChain.h.
bool KinematicChain::jointsMoved [private] |
Indicates whether any of the joints in the chain have moved since the last update of the kinematics.
Definition at line 106 of file kinematicChain.h.
std::vector<Joint *> KinematicChain::jointVec [private] |
A vector of pointers to the joints in the chain.
Definition at line 83 of file kinematicChain.h.
int* KinematicChain::lastJoint [private] |
For any link, gives us the number of the last joint in the chain that affects its pose.
An array where each element corresponds to a link in the chain and the value is the index of the last joint affecting the pose of that link.
Definition at line 91 of file kinematicChain.h.
std::vector<Link *> KinematicChain::linkVec [private] |
A vector of pointers to the links in the chain.
Definition at line 86 of file kinematicChain.h.
int KinematicChain::numChildren [private] |
The number of child robots connected to the chain.
Definition at line 115 of file kinematicChain.h.
int KinematicChain::numDOF [private] |
The number of degrees of freedom in the chain.
Definition at line 74 of file kinematicChain.h.
int KinematicChain::numJoints [private] |
The number of joints in the chain.
Definition at line 77 of file kinematicChain.h.
int KinematicChain::numLinks [private] |
The number of links in the chain.
Definition at line 80 of file kinematicChain.h.
Robot* KinematicChain::owner [private] |
Pointer to the robot that this chain is a part of.
Definition at line 63 of file kinematicChain.h.
transf KinematicChain::tran [private] |
The base transform of the chain relative to the robot base.
Definition at line 94 of file kinematicChain.h.
