Exemple #1
0
//----------------------------------------------------
void TaskObject::setJoints(boost::shared_ptr<std::vector< hardware_interface::JointHandle > > joints)
{
    joints_ = joints;
    updateJointMap();

    jacobian_->resize(6,joints_->size());
    jacobian_->setZero();
}
Exemple #2
0
//----------------------------------------------------
void TaskObject::setChain(boost::shared_ptr<KDL::Chain> chain, std::string root)
{
    chain_ = chain;
    root_ = root;
    updateJointMap();

    fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(*chain_));
    j_solver_.reset(new KDL::ChainJntToJacSolver(*chain_));

    chain_jacobian_->resize(joint_map_->rows());
    link_ = chain_->segments.back().getName();
}
Exemple #3
0
//----------------------------------------------------
TaskObject::TaskObject(unsigned int id, boost::shared_ptr<KDL::Chain> chain, std::string root, boost::shared_ptr<std::vector< hardware_interface::JointHandle > > joints) : id_(id), chain_(chain), root_(root), joints_(joints)
{
    ROS_ASSERT(id_ >= 0);
    trans_l_r_.reset(new Eigen::Affine3d);
    geometries_.reset(new std::vector<boost::shared_ptr<TaskGeometry> >);
    fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(*chain_));
    j_solver_.reset(new KDL::ChainJntToJacSolver(*chain_));

    updateJointMap();

    jacobian_.reset(new Eigen::MatrixXd(6,joints_->size()));
    jacobian_->setZero();
    chain_jacobian_.reset(new KDL::Jacobian(joint_map_->rows()));

    if(chain_->getNrOfSegments() == 0)
        link_ = root_;
    else
        link_ = chain_->segments.back().getName();
}
Exemple #4
0
void ResourceWindow::updateGUI()
{

	SmartBody::SBScene* scene = SmartBody::SBScene::getScene();
	SmartBody::SBAssetManager* assetManager = scene->getAssetManager();

	resourceTree->sortorder(FL_TREE_SORT_ASCENDING);	
	// update path tree	
	updatePath(getTreeFromName("script path"), SmartBody::SBScene::getScene()->getAssetPaths("script"));	
	updatePath(getTreeFromName("motion path"), SmartBody::SBScene::getScene()->getAssetPaths("motion"));	
	updatePath(getTreeFromName("audio path"), SmartBody::SBScene::getScene()->getAssetPaths("audio"));	
	updatePath(getTreeFromName("mesh path"), SmartBody::SBScene::getScene()->getAssetPaths("mesh"));	
	

	// update sequence file list

	const std::vector<std::string> scriptPaths = SmartBody::SBScene::getScene()->getAssetPaths("script");
	resourceTree->clear_children(getTreeFromName("scriptfiles"));
	for (size_t p = 0; p < scriptPaths.size(); p++)
	{
		updateScriptFiles(getTreeFromName("scriptfiles"), scriptPaths[p]);
	}	

	// update runtime scripts
	resourceTree->clear_children(getTreeFromName("script"));
	std::vector<std::string> scriptName = scene->getScriptNames();
	for (size_t i = 0; i < scriptName.size(); i++)
	{
		SmartBody::SBScript* script = scene->getScript(scriptName[i]);
		updateScript(getTreeFromName("script"), script);
	}

	// update skeleton
	resourceTree->clear_children(getTreeFromName("skeleton"));
	std::vector<std::string> skeletons = scene->getSkeletonNames();
	for (size_t c = 0; c < skeletons.size(); c++)
	{
		SmartBody::SBSkeleton * skel = scene->getSkeleton(skeletons[c]);
		updateSkeleton(getTreeFromName("skeleton"), skel);
	}

	// update joint maps
	resourceTree->clear_children(getTreeFromName("jointmap"));	
	SmartBody::SBJointMapManager* jointMapManager = scene->getJointMapManager();
	std::vector<std::string> jointMapNames = jointMapManager->getJointMapNames();
	for (std::vector<std::string>::iterator iter = jointMapNames.begin();
		 iter != jointMapNames.end(); 
		 iter++)
	{
		Fl_Tree_Item* boneMapItem = resourceTree->add(getTreeFromName("jointmap"), (*iter).c_str());
		updateJointMap(boneMapItem, jointMapManager->getJointMap((*iter)));
	}

	// update gesture maps
	resourceTree->clear_children(getTreeFromName("gesturemap"));
	SmartBody::SBGestureMapManager* gestureMapManager = scene->getGestureMapManager();
	std::vector<std::string> gestureMapNames = gestureMapManager->getGestureMapNames();
	for (std::vector<std::string>::iterator iter = gestureMapNames.begin();
		 iter != gestureMapNames.end(); 
		 iter++)
	{
		Fl_Tree_Item* gestureMapItem = resourceTree->add(getTreeFromName("gesturemap"), (*iter).c_str());
		updateGestureMap(gestureMapItem, gestureMapManager->getGestureMap((*iter)));
	}

	// update motion map
	resourceTree->clear_children(getTreeFromName("motion"));
	std::vector<std::string> motionNames = scene->getMotionNames();
	for (size_t i = 0; i < motionNames.size(); i++)
	{
		//resourceTree->add(treeItemList[ITEM_MOTION],mi->first.c_str());
		SmartBody::SBMotion * motion = scene->getMotion(motionNames[i]);
		updateMotion(getTreeFromName("motion"), motion);
	}

	SmartBody::SBAnimationBlendManager* blendManager = scene->getBlendManager();
	// update animation blend map
	resourceTree->clear_children(getTreeFromName("blend"));
	std::vector<std::string> blendNames = blendManager->getBlendNames();
	for (size_t i = 0; i < blendNames.size(); i++)
	{
		//resourceTree->add(treeItemList[ITEM_MOTION],mi->first.c_str());
		SmartBody::SBAnimationBlend * blend = blendManager->getBlend(blendNames[i]);
		updateAnimationBlend(getTreeFromName("blend"), blend);
	}

	// update blend transition map
	resourceTree->clear_children(getTreeFromName("transition"));
	std::vector<std::string> transitionNames = blendManager->getTransitionNames();
	for (size_t i = 0; i < transitionNames.size(); i++)
	{
		//resourceTree->add(treeItemList[ITEM_MOTION],mi->first.c_str());
		SmartBody::SBAnimationTransition * transition = blendManager->getTransitionByName(transitionNames[i]);
		updateBlendTransition(getTreeFromName("transition"), transition);
	}

	// update mesh map
	resourceTree->clear_children(getTreeFromName("mesh"));
	std::vector<std::string> meshNames = assetManager->getDeformableMeshNames();
	for (size_t i = 0; i < meshNames.size(); i++)
	{
		DeformableMesh* mesh = assetManager->getDeformableMesh(meshNames[i]);
		Fl_Tree_Item* meshItem = resourceTree->add(getTreeFromName("mesh"), mesh->getName().c_str());
		updateMesh(meshItem, mesh);
	}


	// update face definition map
	resourceTree->clear_children(getTreeFromName("facedefinition"));
	std::vector<std::string> faceNames = scene->getFaceDefinitionNames();
	for (size_t i = 0; i < faceNames.size(); i++)
	{
		//resourceTree->add(treeItemList[ITEM_MOTION],mi->first.c_str());
		SmartBody::SBFaceDefinition * face = scene->getFaceDefinition(faceNames[i]);
		if (!face)
			continue;
		Fl_Tree_Item* faceTree = resourceTree->add(getTreeFromName("facedefinition"), face->getName().c_str());
		updateFaceDefinition(faceTree, face);
	}

	// update event handler list
	SmartBody::SBEventManager* eventManager = SmartBody::SBScene::getScene()->getEventManager();
	std::map<std::string, SmartBody::SBEventHandler*>& eventMap = eventManager->getEventHandlers();
	std::map<std::string, SmartBody::SBEventHandler*>::iterator ei;
	resourceTree->clear_children(getTreeFromName("eventhandler"));
	for ( ei  = eventMap.begin();
		  ei != eventMap.end();
		  ei++)
	{
		updateEventHandler(getTreeFromName("eventhandler"), ei->second);
	}
	// Below are instance objects :

	// update pawn objects
	resourceTree->clear_children(getTreeFromName("pawn"));
	const std::vector<std::string>& pawnNames = scene->getPawnNames();
	for (size_t i = 0; i < pawnNames.size(); i++)
	{
		SmartBody::SBPawn* pawn = scene->getPawn(pawnNames[i]);
		updatePawn(getTreeFromName("pawn"), pawn);
	}

	// update characters
	resourceTree->clear_children(getTreeFromName("character"));
	const std::vector<std::string>& charNames = scene->getCharacterNames();
	for (size_t i = 0; i < charNames.size(); i++)
	{
		SmartBody::SBCharacter* character = scene->getCharacter(charNames[i]);
		resourceTree->sortorder(FL_TREE_SORT_ASCENDING);
		updateCharacter(getTreeFromName("character"), character);
	}

	
// 	for (SBPhysicsObjMap::iterator iter = phySim->getPhysicsObjMap().begin();
// 		 iter != phySim->getPhysicsObjMap().end();
// 		 iter++)
// 	{
// 		SBPhysicsObj* obj = (*iter).second;
// 		if (dynamic_cast<SbmJointObj*>(obj) == NULL)
// 		{
// 
// 		}
// 	}

	// update services
	SmartBody::SBServiceManager* serviceManager = scene->getServiceManager();
	std::map<std::string, SmartBody::SBService*>& serviceMap = serviceManager->getServices();

	resourceTree->clear_children(getTreeFromName("service"));
	for (std::map<std::string, SmartBody::SBService*>::iterator iter = serviceMap.begin();
		iter != serviceMap.end();
		iter++)
	{
		SmartBody::SBService* service = (*iter).second;
		resourceTree->sortorder(FL_TREE_SORT_ASCENDING);	
		SmartBody::SBPhysicsManager* phyManager = dynamic_cast<SmartBody::SBPhysicsManager*>(service);
		if (phyManager)
			updatePhysicsManager(getTreeFromName("service"),phyManager);
		else
			updateService(getTreeFromName("service"), service);
	}

	// update behavior sets
	resourceTree->clear_children(getTreeFromName("behaviorset"));
	std::map<std::string, SmartBody::SBBehaviorSet*>& behaviorSets = scene->getBehaviorSetManager()->getBehaviorSets();
	for (std::map<std::string, SmartBody::SBBehaviorSet*>::iterator iter = behaviorSets.begin();
		 iter != behaviorSets.end();
		 iter++)
	{
		SmartBody::SBBehaviorSet* behaviorSet = (*iter).second;
		updateBehaviorSet(getTreeFromName("behaviorset"), behaviorSet);
	}


	_dirty = false;

	if (_firstTime)
	{
		hideTree();
		_firstTime = false;
	}
}