Example #1
0
bool RobotNode::initialize(SceneObjectPtr parent, const std::vector<SceneObjectPtr> &children)
{
	RobotPtr rob = robot.lock();
	THROW_VR_EXCEPTION_IF(!rob, "Could not init RobotNode without robot" );

	// robot
	if (!rob->hasRobotNode(static_pointer_cast<RobotNode>(shared_from_this())))
		rob->registerRobotNode(static_pointer_cast<RobotNode>(shared_from_this()));

	// update visualization of coordinate systems
	if (visualizationModel && visualizationModel->hasAttachedVisualization("CoordinateSystem"))
	{
		VisualizationNodePtr v = visualizationModel->getAttachedVisualization("CoordinateSystem");
		// not needed any more!
		// this is a little hack: The globalPose is used to set the "local" position of the attached Visualization:
		// Since the attached visualizations are already positioned at the global pose of the visualizationModel, 
		// we just need the local postJointTransform
		//v->setGlobalPose(postJointTransformation);
	}

    checkValidRobotNodeType();

	for (size_t i=0;i<sensors.size();i++)
	{
		sensors[i]->initialize(shared_from_this());
	}

	return SceneObject::initialize(parent,children);	
}
Example #2
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;
}
Example #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();
}
Example #4
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;
}