Exemplo n.º 1
0
bool RobotConfig::setConfig( RobotNodePtr node, float value )
{
	THROW_VR_EXCEPTION_IF(!node,"Null data");
	
	RobotPtr r = robot.lock();
	if (!r)
	{
		VR_WARNING << "Robot is already deleted, skipping update..." << endl;
		return false;
	}
	THROW_VR_EXCEPTION_IF(!r->hasRobotNode(node),"RobotNode with name " << r->getName() << " does not belong to robot " << r->getName());

	configs[node] = value;
	return true;
}
Exemplo n.º 2
0
void RobotWorld::deleteRobot( 	RobotPtr aRobot,
								bool aNotifyObservers /*= true*/)
{
	auto i = std::find_if( robots.begin(), robots.end(), [aRobot](RobotPtr r)
	{
		return aRobot->getName() == r->getName();
	});
	if (i != robots.end())
	{
		robots.erase( i);
		if (aNotifyObservers == true)
		{
			notifyObservers();
		}
	}
}
Exemplo n.º 3
0
Eigen::Matrix4f LinkedCoordinate::getCoordinateTransformation(const RobotNodePtr &origin, 
		const RobotNodePtr &destination, const RobotPtr &robot){
	
	if (!destination) 
		THROW_VR_EXCEPTION(format("Destination RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
	if (!origin) 
		THROW_VR_EXCEPTION(format("Origin RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
	if (!robot) 
		THROW_VR_EXCEPTION(format("RobotPtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
	if (!robot->hasRobotNode( origin) ) 
		THROW_VR_EXCEPTION(format("Origin robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % origin->getName() % robot->getName() % BOOST_CURRENT_FUNCTION);
	if (!robot->hasRobotNode( destination) ) 
		THROW_VR_EXCEPTION(format("Destination robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % destination->getName() % robot->getName() % BOOST_CURRENT_FUNCTION);
	
//	std::cout << "Destination: " << destination->getName() <<std::endl << "Origin: " << origin->getName() << std::endl;
//	std::cout << "Destination:\n" << destination->getGlobalPose() <<std::endl << "Origin:\n" << origin->getGlobalPose() << std::endl;
	return destination->getGlobalPose().inverse() * origin->getGlobalPose();
}