예제 #1
0
RobotNodeSetPtr RobotNodeSet::createRobotNodeSet(RobotPtr robot, 
												 const std::string &name, 
												 const std::vector< std::string > &robotNodeNames, 
												 const std::string &kinematicRootName, 
												 const std::string &tcpName,
												 bool registerToRobot)
{
	VR_ASSERT (robot!=NULL);
	std::vector< RobotNodePtr > robotNodes;
	if (robotNodeNames.empty())
	{
		VR_WARNING << " No robot nodes in set..." << endl;
	}
	else
	{
		for (unsigned int i=0;i<robotNodeNames.size();i++)
		{
			RobotNodePtr node = robot->getRobotNode(robotNodeNames[i]);
			THROW_VR_EXCEPTION_IF(!node, "No robot node with name " << robotNodeNames[i] << " found...");
			robotNodes.push_back(node);
		}
	}

	RobotNodePtr kinematicRoot;
	if (!kinematicRootName.empty())
	{
		RobotNodePtr node = robot->getRobotNode(kinematicRootName);
		THROW_VR_EXCEPTION_IF(!node, "No root node with name " << kinematicRootName << " found...");
		kinematicRoot = node;
	}
	else
	{
		if (!robotNodes.empty())
			kinematicRoot = robotNodes[0];
	}

	RobotNodePtr tcp;
	if (!tcpName.empty())
	{
		RobotNodePtr node = robot->getRobotNode(tcpName);
		THROW_VR_EXCEPTION_IF(!node, "No tcp node with name " << tcpName << " found...");
		tcp = node;
	}
	else
	{
		if (!robotNodes.empty())
			tcp = robotNodes[robotNodes.size()-1];
	}

	RobotNodeSetPtr rns = RobotNodeSet::createRobotNodeSet(robot, name, robotNodes, kinematicRoot, tcp, registerToRobot);
	return rns;
}
예제 #2
0
void RobotNode::updatePose(bool updateChildren)
{
	THROW_VR_EXCEPTION_IF(!initialized, "Not initialized");

	updateTransformationMatrices();

	// update collision and visualization model and children
	SceneObject::updatePose(updateChildren);

	// apply propagated joint values
	if (propagatedJointValues.size()>0)
	{
		RobotPtr r = robot.lock();
		std::map< std::string, float>::iterator it = propagatedJointValues.begin();
		while (it!=propagatedJointValues.end())
		{
			RobotNodePtr rn = r->getRobotNode(it->first);
			if (!rn)
			{
				VR_WARNING << "Could not propagate joint value from " << name << " to " << it->first << " because dependent joint does not exist...";
			} else
			{
				rn->setJointValue(jointValue*it->second);
			}
			it++;
		}
	}
}
예제 #3
0
bool RobotConfig::setJointValues( RobotPtr r )
{
	if (!r)
		return false;
	WriteLockPtr lock = r->getWriteLock();

	std::map < std::string, float > jv = getRobotNodeJointValueMap();
	std::map< std::string, float >::const_iterator i = jv.begin();

	// first check if all nodes are present
	while (i != jv.end())
	{
		if (!r->hasRobotNode(i->first))
			return false;
		i++;
	}

	// apply jv
	i = jv.begin();
	while (i != jv.end())
	{
		RobotNodePtr rn = r->getRobotNode(i->first);
		if (!rn)
			return false;
		rn->setJointValueNoUpdate(i->second);
		i++;
	}
	r->applyJointValues();
	return true;
}
예제 #4
0
bool RobotConfig::setConfig( const std::string &node, float value )
{
	RobotPtr r = robot.lock();
	if (!r)
	{
		VR_WARNING << "Robot is already deleted, skipping update..." << endl;
		return false;
	}
	RobotNodePtr rn = r->getRobotNode(node);
	if (!rn)
	{
		VR_WARNING << "Did not find robot node with name " << node << endl;
		return false;
	}
	configs[rn] = value;
	return true;
}
예제 #5
0
float RobotConfig::getConfig( const std::string & name ) const
{
	if (!hasConfig(name))
		return 0.0f;
	RobotPtr r = robot.lock();
	if (!r)
	{
		VR_WARNING << "Robot is already deleted..." << endl;
		return 0.0f;
	}
	RobotNodePtr rn = r->getRobotNode(name);
	THROW_VR_EXCEPTION_IF(!rn,"Did not find robot node with name " << name);
	std::map< RobotNodePtr, float >::const_iterator i = configs.find(rn);
	if (i==configs.end())
	{
		VR_ERROR << "Internal error..." << endl;
		return 0.0f;
	}
	return i->second;
}
예제 #6
0
RobotConfigPtr RobotConfig::clone( RobotPtr newRobot )
{
	if (!newRobot)
		newRobot = robot.lock();
	VR_ASSERT (newRobot);
	
	std::map< RobotNodePtr, float > newConfigs;
	std::map< RobotNodePtr, float >::iterator i = configs.begin();
	while (i!=configs.end())
	{
		RobotNodePtr rn = newRobot->getRobotNode(i->first->getName());
		if (!rn)
		{
			VR_WARNING << "Could not completely clone RobotConfig " << name << " because new robot does not know a RobotNode with name " << i->first->getName() << endl;
		} else
		{
			newConfigs[rn] = i->second;
		}
		i++;
	}
	RobotConfigPtr result(new RobotConfig(newRobot,name,newConfigs));
	return result;
}