//----------------------------------------------------
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;
}
}