//---------------------------------------------------- void TaskObject::setJoints(boost::shared_ptr<std::vector< hardware_interface::JointHandle > > joints) { joints_ = joints; updateJointMap(); jacobian_->resize(6,joints_->size()); jacobian_->setZero(); }
//---------------------------------------------------- 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(); }
//---------------------------------------------------- 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(); }
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; } }