Exemplo n.º 1
0
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();
}
Exemplo n.º 2
0
const Joint& Robot::const_joint(size_t jointIndex) const
{
    if(jointIndex < nJoints())
        return *joints_[jointIndex];

    Joint* invalidJoint = new Joint;
    invalidJoint->name("invalid");
    return *invalidJoint;
}
Exemplo n.º 3
0
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;
}
Exemplo n.º 4
0
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;
}
Exemplo n.º 5
0
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;
}
Exemplo n.º 6
0
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);
		}
	}
}
Exemplo n.º 7
0
	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);
	}