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);
}
