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);
}
}
}
}
}
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;
}
float PicoBorgRev::getMotor2()
{
return getMotor(PBR_COMMAND_GET_B);
}
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);
}
}
}
}
}