// move to a target point double MyController::goToObj(Vector3d pos, double velocity, double range, double now) { // get own position Vector3d myPos; m_my->getPosition(myPos); // vector from own position to a target position pos -= myPos; pos.y(0); // distance to a target position double distance = pos.length() - range; // calcurate veloocity from radius of wheels double vel = m_radius*velocity; // start moving m_my->setWheelVelocity(velocity, velocity); // calcurate time of arrival double time = distance / vel; return now + time; }
// object まで移動 double MyController::goToObj(Vector3d nextPos, double velocity, double range, double now) { printf("goToObj関数内 goToObj %lf %lf %lf \n", nextPos.x(), nextPos.y(), nextPos.z()); // 自分の位置の取得 Vector3d myPos; m_my->getPosition(myPos); printf("goToObj関数内 ロボットの現在位置: x: %lf, z %lf \n", myPos.x(), myPos.z()); // 自分の位置からターゲットを結ぶベクトル Vector3d pos = nextPos; pos -= myPos; // y方向は考えない pos.y(0); // 距離計算 double distance = pos.length() - range; //printf("distance: %lf \n", distance); //printf("range = %lf \n", range); // 車輪の半径から移動速度を得る double vel = m_radius*velocity; // 移動開始 m_my->setWheelVelocity(velocity, velocity); // 到着時間取得 double time = distance / vel; return now + time; }
// object まで移動 double MyController::goToObj(Vector3d pos, double velocity, double range, double now) { // 自分の位置の取得 Vector3d myPos; m_my->getPosition(myPos); // 自分の位置からターゲットを結ぶベクトル pos -= myPos; // y方向は考えない pos.y(0); // 距離計算 double distance = pos.length() - range; // 車輪の半径から移動速度を得る double vel = m_radius*velocity; // 移動開始 m_my->setWheelVelocity(velocity, velocity); // 到着時間取得 double time = distance / vel; return now + time; }
void MyController::onInit(InitEvent &evt) { m_my = getRobotObj(myname()); // 初期位置取得 m_my->getPosition(m_inipos); // 車輪の半径と車輪間隔距離 m_radius = 10.0; m_distance = 10.0; m_time = 0.0; // 車輪の半径と車輪間距離設定 m_my->setWheel(m_radius, m_distance); m_state = 0; // ここではゴミの名前が分かっているとします m_trashes.push_back("can_0"); m_trashes.push_back("can_1"); m_trashes.push_back("petbottle_0"); srand((unsigned)time( NULL )); // 車輪の回転速度 m_vel = 0.3; // 関節の回転速度 m_jvel = 0.6; }
bool MyController::recognizeNearestTrash(Vector3d &pos, std::string &name) { ///////////////////////////////////////////// ///////////ここでゴミを認識します//////////// ///////////////////////////////////////////// // 候補のゴミが無い場合 if(m_trashes.empty()){ return false; } /*else { printf("m_trashes.size(): %d \n", m_trashes.size()); }*/ bool found = false; // 自分の位置の取得 Vector3d myPos; m_my->getPosition(myPos); // もっと近いゴミを検索する double dis_min = 10000000000000.0; double distance; double min_idx; // 現位置に検出出来るゴミを検索する for(int trashNum = 0; trashNum < m_trashes.size(); trashNum++) { name = m_trashes[trashNum]; if(getObj(name.c_str())) { SimObj *trash = getObj(name.c_str()); //printf("created SimObj Trash \n"); // ゴミの位置取得 trash->getPosition(pos); //printf("ゴミの位置: %lf %lf %lf \n", pos.x(), pos.y(), pos.z()); distance = (myPos.x() - pos.x()) * (myPos.x() - pos.x()) + (myPos.z() - pos.z()) * (myPos.z() - pos.z()); if (distance < dis_min) { min_idx = trashNum; dis_min = distance; } found = true; } else { //printf("cannot find obj with such name \n"); } } if (found == true) { name = m_trashes[min_idx]; printf("nearestObj trash ^^^: %s \n", name.c_str()); SimObj *trash = getObj(name.c_str()); // ゴミの位置取得 trash->getPosition(pos); } else { return false; } return found; }
void MyController::setRobotPosition(double x, double z) { Vector3d myPos; m_my->getPosition(myPos); double y = myPos.y(); m_my->setPosition(x, y, z); return; }
//定期的に呼び出される関数です。 double CameraController::onAction(ActionEvent &evt) { // ロボット位置取得 r_my->getPosition(r_pos); // カメラ番号取得 m_my = getObj(myname()); m_name = m_my->name(); // 定点カメラをロボットの方向に回転 rotateTowardRobot(r_pos); return 0.05; }
void CameraController::onInit(InitEvent &evt) { // カメラ初期方向 1:45 2:135 3:315 4:225 // ロボット初期位置取得 r_my = getRobotObj("robot_004"); r_my->getPosition(r_pos); // カメラ番号取得 m_my = getObj(myname()); m_name = m_my->name(); // 定点カメラをロボットの方向に回転 rotateTowardRobot(r_pos); }
char* MyController::sendSceneInfo(std::string header, int camID) { m_my->setWheelVelocity(0.0, 0.0); Vector3d myPos; m_my->getPosition(myPos); double x = myPos.x(); double z = myPos.z(); double theta = calcHeadingAngle(); // y方向の回転は無しと考える // カメラがついているリンク名取得 std::string link = ""; //"WAIST_LINK0"; //m_my->getCameraLinkName(3); if (camID < 3) { link = m_my->getCameraLinkName(camID); } else { link = "WAIST_LINK0"; } //const dReal *lpos = m_my->getParts(link.c_str())->getPosition(); Vector3d lpos; m_my->getParts(link.c_str())->getPosition(lpos); // カメラの位置取得(リンク座標系) Vector3d cpos; m_my->getCamPos(cpos, camID); // カメラの位置(絶対座標系, ロボットの回転はないものとする) printf("linkpos: %lf %lf %lf \n", lpos.x(), lpos.y(), lpos.z()); printf("camerapos: %lf %lf %lf \n", cpos.x(), cpos.y(), cpos.z()); Vector3d campos(lpos.x() + cpos.z() * sin(DEG2RAD(theta)), lpos.y() + cpos.y(), lpos.z() + cpos.z() * cos(DEG2RAD(theta))); //Vector3d campos(cpos.x(), cpos.y(), cpos.z()); // カメラの方向取得(ロボットの回転,関節の回転はないものとする) Vector3d cdir; m_my->getCamDir(cdir, camID); char *replyMsg = new char[1024]; sprintf(replyMsg, "%s %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf", header.c_str(), x, z, theta, campos.x(), campos.y(), campos.z(), cdir.x(), cdir.y(), cdir.z()); printf("%s \n", replyMsg); m_srv->sendMsgToSrv(replyMsg); m_my->setWheelVelocity(0.0, 0.0); return replyMsg; }
//定期的に呼び出される関数です。 double CameraController::onAction(ActionEvent &evt) { // ロボット位置取得 r_my->getPosition(r_pos); // カメラ番号取得 m_my = getObj(myname()); m_name = m_my->name(); // std::cout << "configuringPath : " << configuringPath << std::endl; if (!configuringPath) { // 定点カメラをロボットの方向に回転 // rotateTowardRobot(r_pos); } return 0.05; }
void CameraController::onInit(InitEvent &evt) { // カメラ初期方向 1:45 2:135 3:315 4:225 // ロボット初期位置取得 r_my = getRobotObj("robot_004"); r_my->getPosition(r_pos); // カメラ番号取得 m_my = getObj(myname()); m_my->getPosition(m_rotatePos); m_pathPos = Vector3d(m_rotatePos.x() -500, m_rotatePos.y() + 1700, m_rotatePos.z()); m_name = m_my->name(); configuringPath = false; // 定点カメラをロボットの方向に回転 // rotateTowardRobot(r_pos); rotateTowardGround(); }
bool MyController::calcGrabPos(Vector3d pos, double robotShoulderWidth, Vector3d &grabPos) { // ロボットの幅の半分 16.5cm robotShoulderWidth = 16.5; //printf("物体の座標 %lf %lf %lf \n", pos.x(), pos.y(), pos.z()); // 物体のある高さを保存暗記する double grabX = 0, grabY = pos.y(), grabZ = 0; // 自分の位置の取得 Vector3d myPos; m_my->getPosition(myPos); //printf("ロボットの位置 %lf %lf %lf \n", myPos.x(), myPos.y(), myPos.z()); // 自分の位置からターゲットを結ぶベクトル pos -= myPos; //printf("結ぶ座標 %lf %lf %lf \n", pos.x(), pos.y(), pos.z()); double dis = sqrt(pos.x() * pos.x() + pos.z() * pos.z()); if(dis > robotShoulderWidth) { // 物体を掴むために、ロボットが結ぶベクトルからズレる角度 double rate = robotShoulderWidth / dis; double ang = asin(rate); //printf("angle: %lf \n", ang * 180 / PI); grabX = cos(ang) * pos.x() + sin(ang) * pos.z(); grabZ = cos(ang) * pos.z() - sin(ang) * pos.x(); //printf("grabX %lf, grabZ %lf, myPos.x %lf, myPos.z %lf \n", grabX, grabZ, myPos.x(), myPos.z()); // ロボットが向くべき座標 grabX += myPos.x(); grabZ += myPos.z(); grabPos.set(grabX, grabY, grabZ); printf("向くべき座標 %lf %lf %lf \n", grabPos.x(), grabPos.y(), grabPos.z()); return true; } else { return false; } return false; }
void MyController::onInit(InitEvent &evt) { m_my = getRobotObj(myname()); // 初期位置取得 m_my->getPosition(m_inipos); //v3の追加点 // ゴミがある方向にカメラを向ける m_my->setCamDir(Vector3d(0.0, -1.0, 1.0),1); m_my->setCamDir(Vector3d(0.0, -1.0, 1.0),2); // 車輪の半径と車輪間隔距離 m_radius = 10.0; m_distance = 10.0; m_time = 0.0; // 車輪の半径と車輪間距離設定 m_my->setWheel(m_radius, m_distance); m_state = 0; srand((unsigned)time( NULL )); // 車輪の回転速度 m_vel = 1.0; //1.0; m_rotateVel = 0.6; //1.0; // 関節の回転速度 m_jvel = 0.6; m_lookObjFlg = 0.0; // grasp初期化 m_grasp = false; m_srv = NULL; m_sended = false; m_executed = false; }
void MyController::onInit(InitEvent &evt) { start = false; elevator = false; crowd = false; end = false; stop = false; first = false; i=0; m_my = getRobotObj(myname()); // 初期位置取得 m_my->getPosition(m_inipos); // 車輪の半径と車輪間隔距離 m_radius = 10.0; m_distance = 10.0; m_time = 0.0; // 車輪の半径と車輪間距離設定 m_my->setWheel(m_radius, m_distance); m_state = 0; srand((unsigned)time( NULL )); // 車輪の回転速度 m_vel = 0.3; // 関節の回転速度 m_jvel = 0.6; }
bool MyController::recognizeRandomTrash(Vector3d &pos, std::string &name) { ///////////////////////////////////////////// ///////////ここでゴミを認識します//////////// ///////////////////////////////////////////// // 候補のゴミが無い場合 if(m_trashes.empty()){ broadcastMsgToSrv("Found all known trashes"); return false; } /*else { printf("m_trashes.size(): %d \n", m_trashes.size()); }*/ bool found = false; // 自分の位置の取得 Vector3d myPos; m_my->getPosition(myPos); // 現位置に検出出来るゴミを検索する // ここでは乱数を使ってゴミを決定します int trashNum = rand() % m_trashes.size(); name = m_trashes[trashNum]; // robot stop if it cannot grab nearest object, so i didnt use find nearest obj function // when use while() loop, robot freeze when all object found // donot use while() loop xxx /*while(!getObj(name.c_str())) { //for(int trashNum = 0; trashNum < m_trashes.size(); trashNum++) { trashNum = rand() % m_trashes.size(); name = m_trashes[trashNum]; }*/ // if cannot find object by random(), iteratate through all the name of trash. int objFindTry = 0; if(!getObj(name.c_str())) { while(objFindTry < m_trashes.size()) { name = m_trashes[objFindTry]; if(getObj(name.c_str())) { found = true; break; } objFindTry++; } } else if(getObj(name.c_str())) { found = true; } if(!found) { //broadcastMsgToSrv("remain known obj not found"); return false; } if(found) { SimObj *trash = getObj(name.c_str()); //printf("created SimObj Trash \n"); // ゴミの位置取得 trash->getPosition(pos); printf("ゴミの位置: %lf %lf %lf \n", pos.x(), pos.y(), pos.z()); } if (found) { //name = m_trashes[min_idx]; printf("random Obj: %s \n", name.c_str()); SimObj *trash = getObj(name.c_str()); // ゴミの位置取得 trash->getPosition(pos); } return found; }
double MyController::onAction(ActionEvent &evt) { if(!checkService("RecogTrash")){ m_srv == NULL; } if(m_srv == NULL){ // ゴミ認識サービスが利用可能か調べる if(checkService("RecogTrash")){ // ゴミ認識サービスに接続 m_srv = connectToService("RecogTrash"); } } //if(evt.time() < m_time) printf("state: %d \n", m_state); switch(m_state) { // 初期状態 case 0: { if(m_srv == NULL){ // ゴミ認識サービスが利用可能か調べる if(checkService("RecogTrash")){ // ゴミ認識サービスに接続 m_srv = connectToService("RecogTrash"); } } else if(m_srv != NULL && m_executed == false){ //rotate toward upper m_my->setJointVelocity("LARM_JOINT4", -m_jvel, 0.0); m_my->setJointVelocity("RARM_JOINT4", -m_jvel, 0.0); // 50°回転 m_time = DEG2RAD(ROTATE_ANG) / m_jvel + evt.time(); m_state = 5; m_executed = false; } break; } case 5: { if(evt.time() > m_time && m_executed == false) { //m_my->setJointVelocity("LARM_JOINT1", 0.0, 0.0); m_my->setJointVelocity("LARM_JOINT4", 0.0, 0.0); m_my->setJointVelocity("RARM_JOINT4", 0.0, 0.0); sendSceneInfo("Start"); //m_srv->sendMsgToSrv("Start"); printf("Started! \n"); m_executed = true; } break; } // 物体の方向が帰ってきた case 20: { // 送られた座標に回転する m_time = rotateTowardObj(nextPos, m_rotateVel, evt.time()); //printf("debug %lf %lf \n", evt.time(), m_time); m_state = 21; m_executed = false; break; } case 21: { // ロボットが回転中 //printf("debug %lf %lf \n", evt.time(), m_time); if(evt.time() > m_time && m_executed == false) { // 物体のある場所に到着したので、車輪と止め、関節を回転し始め、物体を拾う m_my->setWheelVelocity(0.0, 0.0); // 物体のある方向に回転した //printf("目的地の近くに移動します %lf %lf %lf \n", nextPos.x(), nextPos.y(), nextPos.z()); // 送られた座標に移動する m_time = goToObj(nextPos, m_vel*4, m_range, evt.time()); //m_time = goToObj(nextPos, m_vel*4, 40, evt.time()); m_state = 22; m_executed = false; } break; } case 22: { // 送られた座標に移動した if(evt.time() > m_time && m_executed == false) { m_my->setWheelVelocity(0.0, 0.0); printf("止める \n"); Vector3d myPos; m_my->getPosition(myPos); double x = myPos.x(); double z = myPos.z(); double theta = 0; // y方向の回転は無しと考える char replyMsg[256]; bool found = recognizeNearestTrash(m_tpos, m_tname); // ロボットのステートを更新 if (found == true) { m_state = 500; m_executed = false; //printf("m_executed = false, state = 500 \n"); } else { //printf("Didnot found anything \n"); m_state = 10; m_executed = false; } } break; } case 30: { // 送られた座標に回転する m_time = rotateTowardObj(nextPos, m_rotateVel, evt.time()); //printf("case 30 time: %lf \n", m_time); m_state = 31; m_executed = false; break; } // 物体を掴むために、ロボットの向く角度をズラス case 31: { if(evt.time() > m_time && m_executed == false) { Vector3d grabPos; //printf("斜めにちょっとずれた以前の時間 time: %lf \n", evt.time()); if(calcGrabPos(nextPos, 20, grabPos)) { m_time = rotateTowardObj(grabPos, m_vel / 5, evt.time()); printf("斜め grabPos :%lf %lf %lf \n", grabPos.x(), grabPos.y(), grabPos.z()); printf("time: %lf \n", m_time); } m_state = 32; m_executed = false; } break; } // 物体の方向が帰ってきた case 32: { if(evt.time() > m_time && m_executed == false) { // 物体のある場所に到着したので、車輪と止め、関節を回転し始め、物体を拾う m_my->setWheelVelocity(0.0, 0.0); //printf("回転を止めた evt.time %lf \n", evt.time()); // 関節の回転を始める m_my->setJointVelocity("RARM_JOINT1", -m_jvel, 0.0); // 50°回転 m_time = DEG2RAD(ROTATE_ANG) / m_jvel + evt.time(); m_state = 33; m_executed = false; } break; } case 33: { // 関節回転中 if(evt.time() > m_time && m_executed == false) { // 関節の回転を止める m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0); // 自分の位置の取得 Vector3d myPos; m_my->getPosition(myPos); double x = myPos.x(); double z = myPos.z(); double theta = 0; // y方向の回転は無しと考える //物体を掴めるか掴めないかによって処理を分岐させる if(m_grasp) { // 物体を掴んだ // 捨てたゴミをゴミ候補 std::vector<std::string>::iterator it; // ゴミ候補を得る it = std::find(m_trashes.begin(), m_trashes.end(), m_tname); // 候補から削除する m_trashes.erase(it); printf("erased ... \n"); // ゴミ箱への行き方と問い合わせする char replyMsg[256]; // もっとも近いゴミ箱を探す //bool found = recognizeNearestTrashBox(m_trashBoxPos, m_trashBoxName); // ゴミを置くべき座標を探す bool found = findPlace2PutObj(m_trashBoxPos, m_tname); if(found) { // ゴミ箱が検出出来た std::cout << "trashboxName " << m_trashBoxName << std::endl; sprintf(replyMsg, "AskTrashBoxRoute %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf", x, z, theta, m_trashBoxPos.x(), m_trashBoxPos.y(), m_trashBoxPos.z()); } else { sprintf(replyMsg, "AskTrashBoxPos %6.1lf %6.1lf %6.1lf", x, z, theta); } m_srv->sendMsgToSrv(replyMsg); m_executed = true; } else { // 物体を掴めなかった、次に探す場所を問い合わせる // ゴミを掴めなかったもしくはゴミが無かった、次にゴミのある場所を問い合わせする // 逆方向に関節の回転を始める m_my->setJointVelocity("RARM_JOINT1", m_jvel, 0.0); // 50°回転 m_time = DEG2RAD(ROTATE_ANG) / m_jvel + evt.time(); m_state = 34; m_lastFailedTrash = m_tname; m_executed = false; } } break; } case 34: { if(evt.time() > m_time && m_executed == false) { // 関節の回転を止める m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0); // 自分の位置の取得 Vector3d myPos; m_my->getPosition(myPos); double x = myPos.x(); double z = myPos.z(); double theta = 0; char replyMsg[256]; sprintf(replyMsg, "AskObjPos %6.1lf %6.1lf %6.1lf", x, z, theta); printf("case 34 debug %s \n", replyMsg); m_srv->sendMsgToSrv(replyMsg); m_executed = true; } break; } case 40: { // 送られた座標に回転する m_time = rotateTowardObj(nextPos, m_rotateVel, evt.time()); m_state = 41; m_executed = false; break; } case 41: { // 送られた座標に回転中 if(evt.time() > m_time && m_executed == false) { // 送られた座標に移動する printf("目的地の近くに移動します %lf %lf %lf \n", nextPos.x(), nextPos.y(), nextPos.z()); m_time = goToObj(nextPos, m_vel*4, m_range, evt.time()); m_state = 42; m_executed = false; } break; } case 42: { // 送られた座標に移動中 if(evt.time() > m_time && m_executed == false) { // 送られた座標に到着した、 自分の位置の取得 Vector3d myPos; m_my->getPosition(myPos); double x = myPos.x(); double z = myPos.z(); double theta = 0; // y方向の回転は無しと考える char replyMsg[256]; // もっとも近いゴミ箱を探す bool found = recognizeNearestTrashBox(m_trashBoxPos, m_trashBoxName); if(found) { // ゴミ箱が検出出来た std::cout << "trashboxName " << m_trashBoxName << std::endl; sprintf(replyMsg, "AskTrashBoxRoute %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf", x, z, theta, m_trashBoxPos.x(), m_trashBoxPos.y(), m_trashBoxPos.z()); } else { sprintf(replyMsg, "AskTrashBoxPos %6.1lf %6.1lf %6.1lf", x, z, theta); } m_srv->sendMsgToSrv(replyMsg); m_executed = true; } break; } case 50: { if(evt.time() > m_time && m_executed == false) { Vector3d throwPos; //printf("斜めにちょっとずれた以前の時間 time: %lf \n", evt.time()); // 送られた座標に到着した、 自分の位置の取得 Vector3d myPos; m_my->getPosition(myPos); printf("robot pos %lf %lf \n", myPos.x(), myPos.z()); // grasp中のパーツを取得します CParts *parts = m_my->getParts("RARM_LINK7"); // grasp中のパーツの座標を取得出来れば、回転する角度を逆算出来る。 Vector3d partPos; if (parts->getPosition(partPos)) { printf("parts pos before rotate %lf %lf %lf \n", partPos.x(), partPos.y(), partPos.z()); } //if(calcGrabPos(nextPos, 20, throwPos)) { if(calcGrabPos(nextPos, 20, throwPos)) { m_time = rotateTowardObj(throwPos, m_vel / 5, evt.time()); printf("斜めに捨てる throwPos :%lf %lf %lf \n", throwPos.x(), throwPos.y(), throwPos.z()); //printf("time: %lf \n", m_time); } m_state = 51; m_executed = false; } break; } case 51: { // ゴミ箱に到着したので、車輪を停止し、アームを下ろし、物体をゴミ箱に捨てる準備をする m_my->setWheelVelocity(0.0, 0.0); // grasp中のパーツを取得します CParts *parts = m_my->getParts("RARM_LINK7"); // grasp中のパーツの座標を取得出来れば、回転する角度を逆算出来る。 Vector3d partPos; if (parts->getPosition(partPos)) { printf("parts pos after rotate %lf %lf %lf \n", partPos.x(), partPos.y(), partPos.z()); } // releaseします parts->releaseObj(); // ゴミが捨てられるまで少し待つ sleep(1); // grasp終了 m_grasp = false; //confirmThrewTrashPos(m_threwPos, m_tname); //printf("捨てた座標: %lf %lf %lf \n", m_threwPos.x(), m_threwPos.y(), m_threwPos.z()); // 関節の回転を始める m_my->setJointVelocity("RARM_JOINT1", m_jvel, 0.0); m_time = DEG2RAD(ROTATE_ANG) / m_jvel + evt.time() + 1.0; m_state = 52; m_executed = false; break; } case 52: { // 関節が回転中 if(evt.time() > m_time && m_executed == false) { // 関節が元に戻った、関節の回転を止める m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0); // 自分の位置の取得 Vector3d myPos; m_my->getPosition(myPos); double x = myPos.x(); double z = myPos.z(); double theta = 0; // y方向の回転は無しと考える // ゴミを捨てたので、次にゴミのある場所を問い合わせする char replyMsg[256]; if(recognizeNearestTrash(m_tpos, m_tname)) { m_executed = false; // 物体が発見された m_state = 500; } else { sprintf(replyMsg, "AskObjPos %6.1lf %6.1lf %6.1lf", x, z, theta); m_srv->sendMsgToSrv(replyMsg); m_executed = true; } } break; } case 100: { m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0); m_my->setWheelVelocity(0.0, 0.0); break; } case 800: { if(evt.time() > m_time && m_executed == false) { sendSceneInfo(); m_executed = true; } break; } case 805: { if(evt.time() > m_time && m_executed == false) { // 送られた座標に移動する double range = 0; m_time = rotateTowardObj(nextPos, m_rotateVel, evt.time()); m_state = 807; m_executed = false; } break; } case 807: { if(evt.time() > m_time && m_executed == false) { m_my->setWheelVelocity(0.0, 0.0); printf("移動先 x: %lf, z: %lf \n", nextPos.x(), nextPos.z()); m_time = goToObj(nextPos, m_vel*4, m_range, evt.time()); m_state = 810; m_executed = false; } break; } case 810: { // 送られた座標に移動中 if(evt.time() > m_time && m_executed == false) { m_my->setWheelVelocity(0.0, 0.0); m_time = rotateTowardObj(m_lookingPos, m_rotateVel, evt.time()); m_executed = false; m_state = 815; } break; } case 815: { // 送られた座標に移動中 if(evt.time() > m_time && m_executed == false) { m_my->setWheelVelocity(0.0, 0.0); sendSceneInfo(); printf("sent data to SIGViewer \n"); m_executed = true; } break; } case 920: { // 送られた座標に回転する m_time = rotateTowardObj(nextPos, m_rotateVel, evt.time()); m_state = 921; m_executed = false; break; } case 921: { // ロボットが回転中 if(evt.time() > m_time && m_executed == false) { m_my->setWheelVelocity(0.0, 0.0); m_executed = false; } break; } default: { break; } } return 0.05; }
double MyController::rotateTowardObj(Vector3d pos, double velocity, double now) { // 自分の位置の取得 Vector3d myPos; m_my->getPosition(myPos); // 自分の位置からターゲットを結ぶベクトル Vector3d tmpp = pos; tmpp -= myPos; // y方向は考えない tmpp.y(0); // 自分の回転を得る Rotation myRot; m_my->getRotation(myRot); printf("ロボットの現在位置: x: %lf, z %lf \n", myPos.x(), myPos.z()); // エンティティの初期方向 Vector3d iniVec(0.0, 0.0, 1.0); // y軸の回転角度を得る(x,z方向の回転は無いと仮定) double qw = myRot.qw(); double qy = myRot.qy(); double theta = 2*acos(fabs(qw)); if(qw*qy < 0) theta = -1*theta; printf("ロボットが向いている角度 current theta: %lf(deg) \n", theta * 180 / PI); // z方向からの角度 double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0)); double targetAngle = acos(tmp); // 方向 if(tmpp.x() > 0) targetAngle = -1*targetAngle; targetAngle += theta; printf("targetAngle: %lf(deg) currentAngle: %lf(deg) \n", targetAngle*180.0/PI, theta * 180.0 / PI); if(targetAngle == 0.0){ return 0.0; } else { // 回転すべき円周距離 double distance = m_distance*PI*fabs(targetAngle)/(2*PI); printf("distance: %lf \n", distance); // 車輪の半径から移動速度を得る double vel = m_radius*velocity; printf("radius: %lf, velocity: %lf, vel: %lf \n", m_radius, velocity, vel); // 回転時間(u秒) double time = distance / vel; printf("rotateTime: %lf = dis: %lf / vel: %lf\n", time, distance, vel); // 車輪回転開始 if(targetAngle > 0.0){ m_my->setWheelVelocity(velocity, -velocity); } else{ m_my->setWheelVelocity(-velocity, velocity); } return now + time; } }
double MyController::rotateTowardObj(Vector3d pos, double velocity, double now) { // get own position Vector3d myPos; m_my->getPosition(myPos); // vector from own position to a target position Vector3d tmpp = pos; tmpp -= myPos; // rotation about y-axis is always 0 tmpp.y(0); // get own rotation Rotation myRot; m_my->getRotation(myRot); // initial direction Vector3d iniVec(0.0, 0.0, 1.0); // get rotation angle about y-axis double qw = myRot.qw(); double qy = myRot.qy(); double theta = 2*acos(fabs(qw)); if(qw*qy < 0){ theta = -theta; } // angle from z-axis double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0)); double targetAngle = acos(tmp); // calcurate target angle if(tmpp.x() > 0){ targetAngle = -1*targetAngle; } targetAngle += theta; if(targetAngle<-M_PI){ targetAngle += 2*M_PI; } else if(targetAngle>M_PI){ targetAngle -= 2*M_PI; } if(targetAngle == 0.0){ return 0.0; } else { // circumference length for rotation double distance = m_distance*M_PI*fabs(targetAngle)/(2*M_PI); // calcurate velocity from radius of wheels double vel = m_radius*velocity; // rotation time (micro second) double time = distance / vel; // start rotating if(targetAngle > 0.0){ m_my->setWheelVelocity(velocity, -velocity); } else{ m_my->setWheelVelocity(-velocity, velocity); } return now + time; } }
double MyController::rotateTowardObj(Vector3d pos, double velocity, double now) { // 自分の位置の取得 Vector3d myPos; m_my->getPosition(myPos); // 自分の位置からターゲットを結ぶベクトル Vector3d tmpp = pos; tmpp -= myPos; // y方向は考えない tmpp.y(0); // 自分の回転を得る Rotation myRot; m_my->getRotation(myRot); // エンティティの初期方向 Vector3d iniVec(0.0, 0.0, 1.0); // y軸の回転角度を得る(x,z方向の回転は無いと仮定) double qw = myRot.qw(); double qy = myRot.qy(); double theta = 2*acos(fabs(qw)); if(qw*qy < 0) theta = -1*theta; // z方向からの角度 double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0)); double targetAngle = acos(tmp); // 方向 if(tmpp.x() > 0) targetAngle = -1*targetAngle; targetAngle += theta; if(targetAngle == 0.0){ return 0.0; } else { // 回転すべき円周距離 double distance = m_distance*PI*fabs(targetAngle)/(2*PI); // 車輪の半径から移動速度を得る double vel = m_radius*velocity; // 回転時間(u秒) double time = distance / vel; // 車輪回転開始 if(targetAngle > 0.0){ m_my->setWheelVelocity(velocity, -velocity); } else{ m_my->setWheelVelocity(-velocity, velocity); } return now + time; } }
double MyController::rotateTowardObj(Vector3d pos, double velocity, double now) { // 自分の回転を得る Rotation myRot; m_my->getRotation(myRot); // y軸の回転角度を得る(x,z方向の回転は無いと仮定) double qw = myRot.qw(); double qy = myRot.qy(); double theta = 2 * acos(fabs(qw)); if (qw * qy < 0) { theta = -1 * theta; } printf("ロボットが向いている角度 current theta: %lf(deg) \n", theta * 180 / PI); // 自分の位置の取得 Vector3d myPos; m_my->getPosition(myPos); printf("ロボットの現在位置: x: %lf, z %lf \n", myPos.x(), myPos.z()); // 自分の位置からターゲットを結ぶベクトル Vector3d tmpPos = pos; tmpPos -= myPos; // 近すぎるなら,回転なし double dis = tmpPos.x() * tmpPos.x() + tmpPos.z() * tmpPos.z(); if (dis < 1.0) { return 0.0; } // z方向からの角度 double rate = tmpPos.x() / tmpPos.z(); double targetAngle = atan(rate); if (tmpPos.z() < 0) { targetAngle += PI; } printf("回転する角度 targetAngle: %lf(deg) 結ぶベクトル tmpPos.x: %lf, tmpPos.z: %lf, rate: %lf \n", targetAngle*180.0/PI, tmpPos.x(), tmpPos.z(), rate); targetAngle -= theta; if (targetAngle > PI) { targetAngle = targetAngle - 2 * PI; } printf("targetAngle: %lf(deg) currentAngle: %lf(deg) \n", targetAngle*180.0/PI, theta * 180.0 / PI); if (targetAngle == 0.0) { printf("donot need to rotate \n"); return 0.0; } else { // 回転すべき円周距離 double distance = m_distance * PI * fabs(targetAngle) / (2 * PI); // 車輪の半径から移動速度を得る double vel = m_radius * velocity; // 回転時間(u秒) double time = distance / vel; printf("rotateTime: %lf \n", time); // 車輪回転開始 if (targetAngle > 0.0) { m_my->setWheelVelocity(-velocity, velocity); } else { m_my->setWheelVelocity(velocity, -velocity); } return now + time; } }
double MyController::rotateTowardGrabPos(Vector3d pos, double velocity, double now) { printf("start rotate %lf \n", now); //自分を取得 SimObj *my = getObj(myname()); //printf("向く座標 %lf %lf %lf \n", pos.x(), pos.y(), pos.z()); // 自分の位置の取得 Vector3d myPos; m_my->getPosition(myPos); //自分の手のパーツを得ます CParts * parts = my->getParts("RARM_LINK7"); Vector3d partPos; parts->getPosition(partPos); // 自分の位置からターゲットを結ぶベクトル Vector3d tmpp = pos; //tmpp -= myPos; tmpp -= partPos; // y方向は考えない tmpp.y(0); // 自分の回転を得る Rotation myRot; m_my->getRotation(myRot); // エンティティの初期方向 Vector3d iniVec(0.0, 0.0, 1.0); // y軸の回転角度を得る(x,z方向の回転は無いと仮定) double qw = myRot.qw(); double qy = myRot.qy(); double theta = 2*acos(fabs(qw)); //printf("qw: %lf theta: %lf \n", qw, theta); if(qw*qy < 0) { //printf("qw * qy < 0 \n"); theta = -1*theta; } // z方向からの角度 //printf("結ぶベクトル 座標 %lf %lf %lf \n", tmpp.x(), tmpp.y(), tmpp.z()); double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0)); //printf("tmp: %lf \n", tmp); double targetAngle = acos(tmp); //printf("targetAngle: %lf ---> %lf \n", targetAngle, targetAngle*180.0/PI); // 方向 //printf("tmpp.x() %lf \n", tmpp.x()); if(tmpp.x() > 0) { targetAngle = -1*targetAngle; //printf("targetAngle: %lf deg \n", targetAngle*180.0/PI); } targetAngle += theta; //printf("targetAngle: %lf <--- %lf \n", targetAngle, theta); //printf("qw: %lf qy: %lf theta: %lf tmp: %lf targetAngle: %lf \n", qw, qy, theta, tmp, targetAngle); if(targetAngle == 0.0){ //printf("donot need rotate \n"); return 0.0; } else { // 回転すべき円周距離 double distance = m_distance*PI*fabs(targetAngle)/(2*PI); // 車輪の半径から移動速度を得る double vel = m_radius*velocity; // 回転時間(u秒) double time = distance / vel; // 車輪回転開始 if(targetAngle > 0.0){ m_my->setWheelVelocity(velocity, -velocity); } else { m_my->setWheelVelocity(-velocity, velocity); } //printf("distance: %lf vel: %lf time: %lf \n", distance, vel, time); printf("rotate time: %lf, time to stop: %lf \n", time, now + time); return now + time; } }
bool RobotController::goTo(Vector3d pos, double rangeToPoint) { double speed; Vector3d ownPosition; my->getPosition(ownPosition); Rotation ownRotation; my->getRotation(ownRotation); double angle = getAngularXonVect(pos, ownPosition); double dist = getDistoObj(pos,ownPosition); double roll = getRoll(ownRotation); if (angle > 3 || angle < -3) angle = M_PI; // error on angle if ((angle-roll)>-ERROR_ANGLE && (angle-roll)<ERROR_ANGLE) { // error on distance if (dist-rangeToPoint < ERROR_DISTANCE && dist-rangeToPoint > -ERROR_DISTANCE) { stopRobotMove(); return true; } else { speed = dist-rangeToPoint; if (dist-rangeToPoint < 5) { if ( dist-rangeToPoint > 0 ) my->setWheelVelocity(1, 1); else my->setWheelVelocity(-1, -1); } else if ( dist-rangeToPoint > 0 ) my->setWheelVelocity(Robot_speed , Robot_speed ); else my->setWheelVelocity(-Robot_speed , -Robot_speed ); return false; } } else { speed = fabs(angle-roll)*4; if (speed/4 > 0.3) if (angle < -M_PI_2 && roll > M_PI_2) my->setWheelVelocity(-MAX_WHEEL_VELOCITY, MAX_WHEEL_VELOCITY); else if (angle > M_PI_2 && roll < -M_PI_2) my->setWheelVelocity(MAX_WHEEL_VELOCITY, -MAX_WHEEL_VELOCITY); else if (angle < roll) my->setWheelVelocity(MAX_WHEEL_VELOCITY, -MAX_WHEEL_VELOCITY); else my->setWheelVelocity(-MAX_WHEEL_VELOCITY, MAX_WHEEL_VELOCITY); else if (angle < -M_PI_2 && roll > M_PI_2) my->setWheelVelocity(-speed, speed); else if (angle > M_PI_2 && roll < -M_PI_2) my->setWheelVelocity(speed, -speed); else if (angle < roll) my->setWheelVelocity(speed, -speed); else my->setWheelVelocity(-speed, speed); return false; } return false; }