Пример #1
0
 void SimEntity::setInitialPose() {
   bool worldAnchor = false;
   if(!control) return;
   if(config.find("rootNode") != config.end()) {
     if(config.find("anchor") != config.end()) {
       if((std::string)config["anchor"] == "world") {
         worldAnchor = true;
       }
     }
     NodeId id = getNode((std::string)config["rootNode"]);
     NodeData myNode = control->nodes->getFullNode(id);
     utils::Quaternion tmpQ(1, 0, 0, 0);
     utils::Vector tmpV;
     if(config.find("position") != config.end()) {
       myNode.pos.x() = config["position"][0];
       myNode.pos.y() = config["position"][1];
       myNode.pos.z() = config["position"][2];
       control->nodes->editNode(&myNode, EDIT_NODE_POS | EDIT_NODE_MOVE_ALL);
     }
     if(config.find("rotation") != config.end()) {
       // check if euler angles or quaternion is provided; rotate around z
       // if only one angle is provided
       switch (config["rotation"].size()) {
       case 1: tmpV[0] = 0;
         tmpV[1] = 0;
         tmpV[2] = config["rotation"][0];
         tmpQ = utils::eulerToQuaternion(tmpV);
         break;
       case 3: tmpV[0] = config["rotation"][0];
         tmpV[1] = config["rotation"][1];
         tmpV[2] = config["rotation"][2];
         tmpQ = utils::eulerToQuaternion(tmpV);
         break;
       case 4: tmpQ.x() = (sReal)config["rotation"][1];
         tmpQ.y() = (sReal)config["rotation"][2];
         tmpQ.z() = (sReal)config["rotation"][3];
         tmpQ.w() = (sReal)config["rotation"][0];
         break;
       }
       myNode.rot = tmpQ;
       control->nodes->editNode(&myNode, EDIT_NODE_ROT | EDIT_NODE_MOVE_ALL);
     }
     if(worldAnchor) {
       control->sim->connectNodes(id, 0);
     }
     // set Joints
     configmaps::ConfigVector::iterator it;
     configmaps::ConfigMap::iterator joint_it;
     for (it = config["poses"].begin(); it!= config["poses"].end(); ++it) {
       if ((std::string)(*it)["name"] == (std::string)config["pose"]) {
         for (joint_it = (*it)["joints"][0].children.begin();
              joint_it!= (*it)["joints"][0].children.end(); ++joint_it) {
           //fprintf(stderr, "setMotorValue: joint: %s, id: %lu, value: %f\n", ((std::string)joint_it->first).c_str(), motorIDMap[joint_it->first], (double)joint_it->second);
           control->motors->setMotorValue(getMotor(joint_it->first),
                                          joint_it->second);
         }
       }
     }
   }
 }
Пример #2
0
std::vector<Motor*> CameraController::initializeMotors() {
    numMotors = parameters->get<int>("Robot.Motor.Number");
    std::cout << getName() << ": Initializing " << numMotors << " motors...";
    std::vector<Motor*> motor;
    for(size_t i=0;i<numMotors;i++){
        std::string motorName("Robot.Motor."+std::to_string(i));
        
        motor.push_back(getMotor(parameters->get<std::string>(motorName)));
        if (!motor[i]) {
            throw std::runtime_error("Device Not Found");
        }
        motor[i]->enableTorqueFeedback(TIME_STEP);
    }

    std::cout << "done." << std::endl;
    return motor;
}
Пример #3
0
float PicoBorgRev::getMotor2()
{
	return getMotor(PBR_COMMAND_GET_B);
}
Пример #4
0
 void SimEntity::setInitialPose(bool reset) {
   bool worldAnchor = false;
   if(!control) return;
   if(config.find("rootNode") != config.end()) {
     if(config.find("anchor") != config.end()) {
       if((std::string)config["anchor"] == "world") {
         worldAnchor = true;
       }
     }
     NodeId id = getNode((std::string)config["rootNode"]);
     NodeData myNode = control->nodes->getFullNode(id);
     utils::Quaternion tmpQ(1, 0, 0, 0);
     utils::Vector tmpV;
     if(config.find("position") != config.end()) {
       myNode.pos.x() = config["position"][0];
       myNode.pos.y() = config["position"][1];
       myNode.pos.z() = config["position"][2];
       control->nodes->editNode(&myNode, EDIT_NODE_POS | EDIT_NODE_MOVE_ALL);
     }
     if(config.find("rotation") != config.end()) {
       // check if euler angles or quaternion is provided; rotate around z
       // if only one angle is provided
       switch (config["rotation"].size()) {
       case 1: tmpV[0] = 0;
         tmpV[1] = 0;
         tmpV[2] = config["rotation"][0];
         tmpQ = utils::eulerToQuaternion(tmpV);
         break;
       case 3: tmpV[0] = config["rotation"][0];
         tmpV[1] = config["rotation"][1];
         tmpV[2] = config["rotation"][2];
         tmpQ = utils::eulerToQuaternion(tmpV);
         break;
       case 4: tmpQ.x() = (sReal)config["rotation"][1];
         tmpQ.y() = (sReal)config["rotation"][2];
         tmpQ.z() = (sReal)config["rotation"][3];
         tmpQ.w() = (sReal)config["rotation"][0];
         break;
       }
       myNode.rot = tmpQ;
       control->nodes->editNode(&myNode, EDIT_NODE_ROT | EDIT_NODE_MOVE_ALL);
     }
     if(worldAnchor) {
       if (reset) {
         fprintf(stderr, "Resetting initial entity pose.\n");
         std::map<unsigned long, std::string>::iterator it;
         JointId anchorJointId = 0;
         for (it=jointIds.begin(); it!=jointIds.end(); ++it) {
           if (it->second == "anchor_"+name) {
             anchorJointId = it->first;
             break;
           }
         }
         SimJoint* anchorjoint = control->joints->getSimJoint(anchorJointId);
         if (anchorjoint != NULL) {
           anchorjoint->reattachJoint();
         }
         else fprintf(stderr, "Could not reset anchor of entity %s.\n", name.c_str());
       }
       else {
         JointData anchorjoint;
         anchorjoint.nodeIndex1 = id;
         anchorjoint.nodeIndex2 = 0;
         anchorjoint.type = JOINT_TYPE_FIXED;
         anchorjoint.name = "anchor_"+name;
         JointId anchorJointId = control->joints->addJoint(&anchorjoint);
         addJoint(anchorJointId, anchorjoint.name);
       }
     }
     // set Joints
     configmaps::ConfigVector::iterator it;
     configmaps::ConfigMap::iterator joint_it;
     for (it = config["poses"].begin(); it!= config["poses"].end(); ++it) {
       if ((std::string)(*it)["name"] == (std::string)config["pose"]) {
         for (joint_it = (*it)["joints"][0].children.begin();
              joint_it!= (*it)["joints"][0].children.end(); ++joint_it) {
           //fprintf(stderr, "setMotorValue: joint: %s, id: %lu, value: %f\n", ((std::string)joint_it->first).c_str(), motorIDMap[joint_it->first], (double)joint_it->second);
           control->motors->setMotorValue(getMotor(joint_it->first),
                                          joint_it->second);
         }
       }
     }
   }
 }