void MultiBodyGraph::addJoint(const Joint& J) { if(J.id() < 0) { std::ostringstream msg; msg << "Joint id must be greater than zero"; throw std::domain_error(msg.str()); } // check that the joint id don't exist if(jointId2Joint_.find(J.id()) != jointId2Joint_.end()) { std::ostringstream msg; msg << "Joint id: " << J.id() << " already exist."; throw std::domain_error(msg.str()); } // check that the joint name don't exist if(jointName2Id_.find(J.name()) != jointName2Id_.end()) { std::ostringstream msg; msg << "Joint name: " << J.name() << " already exist."; throw std::domain_error(msg.str()); } joints_.push_back(std::make_shared<Joint>(J)); jointId2Joint_[J.id()] = joints_.back(); jointName2Id_[J.name()] = J.id(); }
const Joint& Robot::const_joint(size_t jointIndex) const { if(jointIndex < nJoints()) return *joints_[jointIndex]; Joint* invalidJoint = new Joint; invalidJoint->name("invalid"); return *invalidJoint; }
Joint& Robot::joint(size_t jointIndex) { if(jointIndex < nJoints()) return *joints_[jointIndex]; cerr << "Invalid joint index: (" << jointIndex << ")" << endl; Joint* invalidJoint = new Joint; invalidJoint->name("invalid"); return *invalidJoint; }
const Joint& Robot::const_joint(string jointName) const { map<string,size_t>::const_iterator j = jointNameToIndex_.find(jointName); if( j != jointNameToIndex_.end() ) return *joints_.at(j->second); Joint* invalidJoint = new Joint; invalidJoint->name("invalid"); return *invalidJoint; }
Joint& Robot::joint(string jointName) { map<string,size_t>::const_iterator j = jointNameToIndex_.find(jointName); if( j != jointNameToIndex_.end() ) return *joints_.at(j->second); cerr << "Invalid joint name: (" << jointName << ")" << endl; Joint* invalidJoint = new Joint; invalidJoint->name("invalid"); return *invalidJoint; }
void SParts::dumpConnectionInfo(int level) { char s[256]; //TODO : Magic number printIndent(level); printf("*** %s ***\n", m_name.c_str()); strcpy(s, m_parentJoint ? m_parentJoint->name() : "NULL"); printIndent(level+1); printf("parentJoint=(%s)\n", s); ChildC::iterator i; for (i=m_children.begin(); i!=m_children.end(); i++) { Child *c = *i; if (c) { Joint *childJ = c->currj; strcpy(s, childJ ? childJ->name() : "NULL"); printIndent(level+1); printf("childJ=(%s)\n", s); Joint *nextJ = c->nextj; strcpy(s, nextJ ? nextJ->name() : "NULL"); printIndent(level+1); printf("nextJ=(%s)\n", s); SParts *childP = c->nextp; strcpy(s, childP ? childP->name() : "NULL"); printIndent(level+1); printf("childP=(%s)\n", s); childP->dumpConnectionInfo(level+2); } } }
JointMessage(string const& kinectId, int32_t userId, Joint const& joint) { address = "/kinect/"+kinectId+"/joint/"+joint.name(); const int numArgs = 8; Arg* myArgs[numArgs] = { new ArgInt32(userId), new ArgFloat(joint.mConfidence), new ArgFloat(joint.mPos.x), new ArgFloat(joint.mPos.y), new ArgFloat(joint.mPos.z), new ArgFloat(joint.mVel.x), new ArgFloat(joint.mVel.y), new ArgFloat(joint.mVel.z) }; args.assign(myArgs, myArgs+numArgs); }