void MyController::onRecvMsg(RecvMsgEvent &evt) { std::string msg = evt.getMsg(); if(msg == "Task_start"){ if(trials==0){ m_state = 100; } else{ m_state = 1100; } broadcastMsg("get message: Task_start"); } else if(msg == "Task_end"){ m_my->setWheelVelocity(0.0, 0.0); m_state = 0; broadcastMsg("get message: Task_end"); trials++; } else if(msg == "Time_over"){ m_my->setWheelVelocity(0.0, 0.0); m_state = 0; } /*else if(m_state == 207 && msg == "leave the elevator"){ broadcastMsg("get message: leave the elevator"); m_state = 208; }*/ }
double DemoRobotController::rotateTowardObj(Vector3d pos) { // "pos" means target position // get own position Vector3d ownPosition; m_robotObject->getPartsPosition(ownPosition,"RARM_LINK2"); // pointing vector for target Vector3d l_pos = pos; l_pos -= ownPosition; // ignore variation on y-axis l_pos.y(0); // get own rotation matrix Rotation ownRotation; m_robotObject->getRotation(ownRotation); // get angles arround y-axis double qw = ownRotation.qw(); double qy = ownRotation.qy(); double theta = 2*acos(fabs(qw)); if(qw*qy < 0) theta = -1.0*theta; // rotation angle from z-axis to x-axis double tmp = l_pos.angle(Vector3d(0.0, 0.0, 1.0)); double targetAngle = acos(tmp); // direction if(l_pos.x() > 0) targetAngle = -1.0*targetAngle; targetAngle += theta; double angVelFac = 3.0; double l_angvel = m_angularVelocity/angVelFac; if(targetAngle == 0.0) { return 0.0; } else { // circumferential distance for the rotation double l_distance = m_distance*M_PI*fabs(targetAngle)/(2.0*M_PI); // Duration of rotation motion (micro second) double l_time = l_distance / (m_movingSpeed/angVelFac); // Start the rotation if(targetAngle > 0.0) { m_robotObject->setWheelVelocity(l_angvel, -l_angvel); } else{ m_robotObject->setWheelVelocity(-l_angvel, l_angvel); } return l_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; }
// 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; }
double DemoRobotController::goToObj(Vector3d pos, double range) { // get own position Vector3d robotCurrentPosition; //m_robotObject->getPosition(robotCurrentPosition); m_robotObject->getPartsPosition(robotCurrentPosition,"RARM_LINK2"); // pointing vector for target Vector3d l_pos = pos; l_pos -= robotCurrentPosition; // ignore y-direction l_pos.y(0); // measure actual distance double distance = l_pos.length() - range; // start moving m_robotObject->setWheelVelocity(m_angularVelocity, m_angularVelocity); // time to be elapsed double l_time = distance / m_movingSpeed; return l_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; }
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 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; } }
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::onAction(ActionEvent &evt) { switch(m_state){ // wait case 0:{ break; } // 1st section case 100: { m_time = rotateTowardObj(Vector3d(-50,60,500),m_vel_rot,evt.time()); m_state = 101; break; } case 101: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-50,60,500), m_vel, 1.0, evt.time()); m_state = 102; } break; } case 102: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-50,60,575), m_vel, 1.0, evt.time()); m_state = 103; } break; } case 103: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = rotateTowardObj(Vector3d(-450,60,575),m_vel_rot,evt.time()); m_state = 104; break; } } case 104: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-450,60,575), m_vel, 1.0, evt.time()); m_state = 105; } break; } case 105: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // wait for a walking person usleep(3200000); m_state = 106; } break; } case 106: { m_time = rotateTowardObj(Vector3d(-525,60,575),m_vel_rot,evt.time()); m_state = 107; break; } case 107: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-525,60,575), m_vel, 1.0, evt.time()); m_state = 200; } break; } // 2nd section case 200: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = rotateTowardObj(Vector3d(-800,60,575),m_vel_rot,evt.time()); m_state = 201; } break; } case 201: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-800,60,575), m_vel, 1.0, evt.time()); m_state = 202; } break; } case 202: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = rotateTowardObj(Vector3d(-800,60,275),m_vel_rot,evt.time()); m_state = 203; } break; } case 203: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-800,60,275), m_vel, 1.0, evt.time()); m_state = 204; } break; } case 204: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = rotateTowardObj(Vector3d(-1100,60,275),m_vel_rot,evt.time()); m_state = 205; } break; } case 205: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-1100,60,275), m_vel, 1.0, evt.time()); m_state = 206; } break; } case 206: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // tell robot entered in an elevator broadcastMsg("Door_close"); m_state = 207; } break; } case 207: { // wait for a while until a door opened sleep(5); m_state = 208; break; } case 208: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = rotateTowardObj(Vector3d(-800,60,275),m_vel_rot,evt.time()); m_state = 209; } break; } case 209: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-800,60,275), m_vel, 1.0, evt.time()); m_state = 210; } break; } case 210: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // std::string msg = "Get_off"; broadcastMsg(msg); // sleep(14); m_state = 300; } break; } // 3rd section case 300: { m_time = rotateTowardObj(Vector3d(-800,60,-200),m_vel_rot,evt.time()); m_state = 301; break; } case 301: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-800,60,-200), m_vel, 1.0, evt.time()); m_state = 302; } break; } case 302: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = rotateTowardObj(Vector3d(-970,60,-200),m_vel_rot,evt.time()); m_state = 303; } break; } case 303: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-970,60,-200), m_vel, 1.0, evt.time()); m_state = 304; } break; } case 304: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = rotateTowardObj(Vector3d(-970,60,-400),m_vel_rot,evt.time()); m_state = 305; } break; } case 305: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-970,60,-400), m_vel, 1.0, evt.time()); m_state = 306; } break; } case 306: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = rotateTowardObj(Vector3d(-800,60,-550),m_vel_rot,evt.time()); m_state = 307; } break; } case 307: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-800,60,-550), m_vel, 1.0, evt.time()); m_state = 900; } break; } // finish line case 900: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = rotateTowardObj(Vector3d(-800,60,-750),m_vel_rot,evt.time()); m_state = 901; } break; } case 901: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-800,60,-750), m_vel, 1.0, evt.time()); m_state = 999; } break; } // 1st section case 1100: { //m_time = rotateTowardObj(Vector3d(-50,60,500),m_vel,evt.time()); m_time = rotateTowardObj(Vector3d(-50,60,-300),m_vel_rot,evt.time()); m_state = 1101; break; } case 1101: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); //m_time = goToObj(Vector3d(-50,60,500), m_vel*20, 1.0, evt.time()); m_time = goToObj(Vector3d(-50,60,-300), m_vel, 1.0, evt.time()); m_state = 1102; } break; } case 1102: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); //m_time = goToObj(Vector3d(-50,60,575), m_vel*20, 1.0, evt.time()); m_time = rotateTowardObj(Vector3d(-300,60,-300),m_vel_rot,evt.time()); m_state = 1103; } break; } case 1103: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); //m_time = rotateTowardObj(Vector3d(-450,60,575),m_vel,evt.time()); m_time = goToObj(Vector3d(-300,60,-300), m_vel, 1.0, evt.time()); m_state = 1104; break; } } case 1104: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = rotateTowardObj(Vector3d(-300,60,575),m_vel_rot,evt.time()); m_state = 1105; } break; } case 1105: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-300,60,575), m_vel, 1.0, evt.time()); m_state = 1106; } break; } case 1106: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = rotateTowardObj(Vector3d(-450,60,575),m_vel_rot,evt.time()); m_state = 1107; } break; } case 1107: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-450,60,575), m_vel, 1.0, evt.time()); m_state = 1108; } break; } case 1108: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // wait for a walking person usleep(3200000); m_state = 1109; } break; } case 1109: { m_time = rotateTowardObj(Vector3d(-525,60,575),m_vel_rot,evt.time()); m_state = 1110; break; } case 1110: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-525,60,575), m_vel, 1.0, evt.time()); m_state = 1200; } break; } // 2nd section case 1200: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = rotateTowardObj(Vector3d(-800,60,575),m_vel_rot,evt.time()); m_state = 1201; } break; } case 1201: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-800,60,575), m_vel, 1.0, evt.time()); m_state = 1202; } break; } case 1202: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = rotateTowardObj(Vector3d(-800,60,275),m_vel_rot,evt.time()); m_state = 1203; } break; } case 1203: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-800,60,275), m_vel, 1.0, evt.time()); m_state = 1204; } break; } case 1204: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = rotateTowardObj(Vector3d(-1100,60,275),m_vel_rot,evt.time()); m_state = 1205; } break; } case 1205: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-1100,60,275), m_vel, 1.0, evt.time()); m_state = 1206; } break; } case 1206: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // tell robot entered in an elevator broadcastMsg("Door_close"); m_state = 1207; } break; } case 1207: { // wait for a while until a door opened sleep(5); m_state = 1208; break; } case 1208: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = rotateTowardObj(Vector3d(-800,60,275),m_vel_rot,evt.time()); m_state = 1209; } break; } case 1209: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-800,60,275), m_vel, 1.0, evt.time()); m_state = 1210; } break; } case 1210: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // std::string msg = "Get_off"; broadcastMsg(msg); // sleep(14); m_state = 1300; } break; } // 3rd section case 1300: { m_time = rotateTowardObj(Vector3d(-800,60,-200),m_vel_rot,evt.time()); m_state = 1301; break; } case 1301: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-800,60,-200), m_vel, 1.0, evt.time()); m_state = 1302; } break; } case 1302: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = rotateTowardObj(Vector3d(-970,60,-200),m_vel_rot,evt.time()); m_state = 1303; } break; } case 1303: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-970,60,-200), m_vel, 1.0, evt.time()); m_state = 1304; } break; } case 1304: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = rotateTowardObj(Vector3d(-970,60,-400),m_vel_rot,evt.time()); m_state = 1305; } break; } case 1305: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-970,60,-400), m_vel, 1.0, evt.time()); m_state = 1306; } break; } case 1306: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = rotateTowardObj(Vector3d(-800,60,-550),m_vel_rot,evt.time()); m_state = 1307; } break; } case 1307: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-800,60,-550), m_vel, 1.0, evt.time()); m_state = 1900; } break; } // finish line case 1900: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = rotateTowardObj(Vector3d(-800,60,-750),m_vel_rot,evt.time()); m_state = 1901; } break; } case 1901: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(Vector3d(-800,60,-750), m_vel, 1.0, evt.time()); m_state = 999; } break; } case 999: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); broadcastMsg("Task_finished"); m_state = 0; } break; } } return 0.05; }
double DemoRobotController::onAction(ActionEvent &evt) { switch(m_state) { case 0: { break; } case 1: { this->stopRobotMove(); break; } case 10: { // go straight a bit m_graspObjectName = m_trashName2; // at first, focusing to m_trashName2:can_0 m_robotObject->setWheelVelocity(m_angularVelocity, m_angularVelocity); m_time = 10.0/m_movingSpeed + evt.time(); // time to be elapsed m_state = 20; break; } case 20: { // direct to the trash if(evt.time() >= m_time && m_state==20) { stopRobotMove(); // at first, stop robot maneuver Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashName2); // get position of trash double l_moveTime = rotateTowardObj(l_tpos); // rotate toward the position and calculate the time to be elapsed. m_time = l_moveTime+evt.time(); m_state = 30; } break; } case 30: { // proceed toward trash if(evt.time() >= m_time && m_state==30) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = goToObj(l_tpos, 75.0); // go toward the position and calculate the time to be elapsed. m_time = l_moveTime+evt.time(); m_state = 40; } break; } case 40: { // get back a bit after colliding with the table if(evt.time() >= m_time && m_state==40) { this->stopRobotMove(); // at first, stop robot maneuver m_robotObject->setWheelVelocity(-m_angularVelocity, -m_angularVelocity); m_time = 20./m_movingSpeed + evt.time(); m_state = 50; } break; } case 50: { // detour: rotate toward relay point 1 if(evt.time() >= m_time && m_state==50) { this->stopRobotMove(); double l_moveTime = rotateTowardObj(m_relayPoint1); m_time = l_moveTime+evt.time(); m_state = 60; } break; } case 60: { // detour: go toward relay point 1 if(evt.time() >= m_time && m_state==60) { this->stopRobotMove(); double l_moveTime = goToObj(m_relayPoint1, 0.0); m_time = l_moveTime+evt.time(); m_state = 70; } break; } case 70: { // rotate toward the position in front of trash if(evt.time() >= m_time && m_state==70) { this->stopRobotMove(); double l_moveTime = rotateTowardObj(m_frontTrash1); m_time = l_moveTime+evt.time(); m_state = 80; } break; } case 80: { // go toward the position in front of trash if(evt.time() >= m_time && m_state==80) { this->stopRobotMove(); double l_moveTime = goToObj(m_frontTrash1, 0.0); m_time = l_moveTime+evt.time(); m_state = 90; } break; } case 90: { // rotate toward the trash if(evt.time() >= m_time && m_state==90) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime+evt.time(); m_state = 100; } break; } case 100: { // prepare the robot arm to grasping the trash if(evt.time() >= m_time && m_state==100) { this->stopRobotMove(); this->neutralizeArms(evt.time()); m_state = 105; } break; } case 105: { // fix robot direction for grasping if(evt.time() >= m_time1 && m_state==105) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4 && m_state==105) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4 && m_state==105) { Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime+evt.time(); m_state = 110; } break; } case 110: { // approach to the trash if(evt.time() >= m_time && m_state==110) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = goToObj(l_tpos, 30.0); m_time = l_moveTime+evt.time(); m_state = 120; } break; } case 120: { // try to grasp trash if(evt.time() >= m_time && m_state==120) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = goGraspingObject(l_tpos); m_time = l_moveTime+evt.time(); m_state = 125; } break; } case 125: { if(evt.time() >= m_time && m_state==125) { m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); this->neutralizeArms(evt.time()); m_state = 130; } break; } case 130: { if(evt.time() >= m_time1 && m_state==130) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4 && m_state==130) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4 && m_state==130) { m_robotObject->setWheelVelocity(-m_angularVelocity, -m_angularVelocity); m_time = 20./m_movingSpeed + evt.time(); m_state = 150; } break; } case 150: { if(evt.time() >= m_time && m_state==150) { this->stopRobotMove(); double l_moveTime = rotateTowardObj(m_frontTrashBox2); m_time = l_moveTime + evt.time(); m_state = 160; } break; } case 160: { if(evt.time() >= m_time && m_state==160) { this->stopRobotMove(); double l_moveTime = goToObj(m_frontTrashBox2,0.0); m_time = l_moveTime + evt.time(); m_state = 161; } break; } case 161: { if(evt.time() >= m_time && m_state==161) { this->stopRobotMove(); this->prepareThrowing(evt.time()); m_state = 165; } break; } case 165: { if(evt.time() >= m_time1 && m_state==165) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4 && m_state==165) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4 && m_state==165) { Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashBoxName2); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime + evt.time(); m_state = 170; } break; } case 170: { if(evt.time() >= m_time && m_state==170) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashBoxName2); double l_moveTime = goToObj(l_tpos, 50.0); m_time = l_moveTime + evt.time(); m_state = 180; } break; } case 180: { if(evt.time() >= m_time && m_state==180) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashBoxName2); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime + evt.time(); m_state = 200; } break; } case 200: { // throw trash and get back a bit if(evt.time() >= m_time && m_state==200) { this->stopRobotMove(); this->throwTrash(); sleep(1); m_robotObject->setWheelVelocity(-m_angularVelocity, -m_angularVelocity); m_time = 50.0/m_movingSpeed + evt.time(); m_state = 225; } break; } case 225: { // recover robot arms if(evt.time() >= m_time && m_state==225) { this->stopRobotMove(); this->neutralizeArms(evt.time()); m_state = 240; } break; } //******************************************************************** case 240: { // go next if(evt.time() >= m_time1 && m_state==240) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4 && m_state==240) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4 && m_state==240) { this->stopRobotMove(); m_graspObjectName = m_trashName1; // set next target double l_moveTime = rotateTowardObj(m_frontTrash2); m_time = l_moveTime + evt.time(); m_state = 250; } break; } case 250: { // approach to neighbor of next target if(evt.time() >= m_time && m_state==250) { this->stopRobotMove(); double l_moveTime = goToObj(m_frontTrash2, 0.0); m_time = l_moveTime + evt.time(); m_state = 260; } break; } case 260: { if(evt.time() >= m_time && m_state==260) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashName1); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime+evt.time(); m_state = 270; } break; } case 270: { // approach to next target if(evt.time() >= m_time && m_state==270) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashName1); double l_moveTime = goToObj(l_tpos, 39.0); m_time = l_moveTime + evt.time(); m_state = 275; } break; } case 275: { if(evt.time() >= m_time && m_state==275) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashName1); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime+evt.time(); m_state = 280; } break; } case 280: { if(evt.time() >= m_time && m_state==280) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashName1); double l_moveTime = goGraspingObject(l_tpos); m_time = l_moveTime+evt.time(); m_state = 290; } break; } case 290: { if(evt.time() >= m_time && m_state==290) { m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); this->neutralizeArms(evt.time()); m_state = 300; } break; } case 300: { if(evt.time() >= m_time1 && m_state==300) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4 && m_state==300) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4 && m_state==300) { m_robotObject->setWheelVelocity(-m_angularVelocity, -m_angularVelocity); m_time = 20./m_movingSpeed + evt.time(); m_state = 310; } break; } case 310: { if(evt.time() >= m_time && m_state==310) { this->stopRobotMove(); double l_moveTime = rotateTowardObj(m_frontTrashBox1); m_time = l_moveTime + evt.time(); m_state = 320; } break; } case 320: { if(evt.time() >= m_time && m_state==320) { this->stopRobotMove(); double l_moveTime = goToObj(m_frontTrashBox1,0.0); m_time = l_moveTime + evt.time(); m_state = 340; } break; } case 340: { if(evt.time() >= m_time && m_state==340) { this->stopRobotMove(); this->prepareThrowing(evt.time()); m_state = 350; } break; } case 350: { if(evt.time() >= m_time1 && m_state==350) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4 && m_state==350) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4 && m_state==350) { Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashBoxName1); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime + evt.time(); m_state = 360; } break; } case 360: { if(evt.time() >= m_time && m_state==360) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashBoxName1); double l_moveTime = goToObj(l_tpos, 50.0); m_time = l_moveTime + evt.time(); m_state = 370; } break; } case 370: { if(evt.time() >= m_time && m_state==370) { this->stopRobotMove(); Vector3d l_tpos; this->recognizeObjectPosition(l_tpos, m_trashBoxName1); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime + evt.time(); m_state = 380; } break; } case 380: { // throw trash and get back a bit if(evt.time() >= m_time && m_state==380) { this->stopRobotMove(); this->throwTrash(); sleep(1); m_robotObject->setWheelVelocity(-m_angularVelocity, -m_angularVelocity); m_time = 50.0/m_movingSpeed + evt.time(); m_state = 390; } break; } case 390: { // recover robot arms if(evt.time() >= m_time && m_state==390) { this->stopRobotMove(); m_state = 0; } break; } } return refreshRateOnAction; }
double RobotController::onAction(ActionEvent &evt) { switch(m_state){ // 初期姿勢を設定 seting initial pose case 0: { //broadcastMsgToSrv("Let's start the clean up task\n"); sendMsg("VoiceReco_Service","Let's start the clean up task\n"); double angL1 =m_my->getJointAngle("LARM_JOINT1")*180.0/(PI); double angL4 =m_my->getJointAngle("LARM_JOINT4")*180.0/(PI); double angR1 =m_my->getJointAngle("RARM_JOINT1")*180.0/(PI); double angR4 =m_my->getJointAngle("RARM_JOINT4")*180.0/(PI); double thetaL1 = -20-angL1; double thetaL4 = -160-angL4; double thetaR1 = -20-angR1; double thetaR4 = -160-angR4; if(thetaL1<0) m_my->setJointVelocity("LARM_JOINT1", -m_jvel, 0.0); else m_my->setJointVelocity("LARM_JOINT1", m_jvel, 0.0); if(thetaL4<0) m_my->setJointVelocity("LARM_JOINT4", -m_jvel, 0.0); else m_my->setJointVelocity("LARM_JOINT4", m_jvel, 0.0); if(thetaR1<0) m_my->setJointVelocity("RARM_JOINT1", -m_jvel, 0.0); else m_my->setJointVelocity("RARM_JOINT1", m_jvel, 0.0); if(thetaR4<0) m_my->setJointVelocity("RARM_JOINT4", -m_jvel, 0.0); else m_my->setJointVelocity("RARM_JOINT4", m_jvel, 0.0); m_time_LA1 = DEG2RAD(abs(thetaL1))/ m_jvel + evt.time(); m_time_LA4 = DEG2RAD(abs(thetaL4))/ m_jvel + evt.time(); m_time_RA1 = DEG2RAD(abs(thetaR1))/ m_jvel + evt.time(); m_time_RA4 = DEG2RAD(abs(thetaR4))/ m_jvel + evt.time(); m_state = 1; break; } // 初期姿勢に移動 moving initial pose case 1: { if(evt.time() >= m_time_LA1) m_my->setJointVelocity("LARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time_LA4) m_my->setJointVelocity("LARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time_RA1) m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time_RA4) m_my->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time_LA1 && evt.time() >= m_time_LA4 && evt.time() >= m_time_RA1 && evt.time() >= m_time_RA4){ // 位置Aの方向に回転を開始します setting position a for rotating //broadcastMsgToSrv("Moving to the table"); m_time = rotateTowardObj(pos_a, m_vel, evt.time()); m_state = 2; } break; } // 位置Aの方向に回転 rotating to position a case 2: { // 回転終了 if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // 位置Aに移動します setting position a for moving m_time = goToObj(pos_a, m_vel*4, 0.0, evt.time()); m_state = 3; } break; } // 位置Aに移動 moving to position a case 3: { // 位置Aに到着 if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // 位置Bの方向に回転を開始します setting position b for rotating m_time = rotateTowardObj(pos_b, m_vel, evt.time()); m_state = 4; } break; } // 位置Bの方向に回転 rotating to position b case 4: { // 回転終了 if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // 位置Bに移動します setting position b for moving m_time = goToObj(pos_b, m_vel*4, 0.0, evt.time()); m_state = 5; } break; } // 位置Bに移動 moving to position b case 5: { // 位置Bに到着 if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // テーブルの方向に回転を開始します setting table position for rotating SimObj *table = getObj("table_0"); Vector3d pos; table->getPosition(pos); m_time = rotateTowardObj(pos, m_vel, evt.time()); m_state = 6; } break; } // テーブルの方向に回転 rotating to table case 6: { // 回転終了 if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // ゴミがある場所と名前を取得します // ゴミが見つからなかった if(!this->recognizeTrash(m_tpos,m_tname)){ //broadcastMsgToSrv("No trash detected"); //broadcastMsgToSrv("Task finished"); sleep(10); } // ゴミが見つかった trash detected else{ //broadcastMsgToSrv("Please show which trash to take\n"); sendMsg("VoiceReco_Service","Please show me which object to take"); m_state = 7; } } break; } // wating to point case 7: { break; } // 物体認識開始 starting object recognition case 8: { // m_tpos object direction on object SimObj *target = this->getObj(m_pointedObject.c_str()); target->getPosition(m_tpos); //broadcastMsgToSrv("Ok I will take it\n"); msg_ob = "I will take " + m_pointedObject ; sendMsg("VoiceReco_Service",msg_ob); // ゴミの方向に回転をはじめる m_time = rotateTowardObj(m_tpos, m_vel, evt.time()); m_state = 9; break; } // ゴミの方向に回転をはじめる setting trash position for rotating case 9: { m_time = rotateTowardObj(m_tpos, m_vel, evt.time()); m_state = 10; break; } // ゴミの方向に回転中 rotating to trash case 10: { // 回転終了 if(evt.time() >= m_time){ // 回転を止める m_my->setWheelVelocity(0.0, 0.0); //ゴミの位置まで移動をはじめる setting trash position for moving m_time = goToObj(m_tpos, m_vel*4, 25.0, evt.time()); m_state = 11; } break; } // ゴミの位置まで移動中 moving to trash case 11: { // 移動終了 if(evt.time() >= m_time){ // 移動を止める m_my->setWheelVelocity(0.0, 0.0); // 関節の回転を始める setting arm for grasping double angR1 =m_my->getJointAngle("RARM_JOINT1")*180.0/(PI); double angR4 =m_my->getJointAngle("RARM_JOINT4")*180.0/(PI); double thetaR1 = -30.0-angR1; double thetaR4 = 0.0-angR4; if(thetaR1<0) m_my->setJointVelocity("RARM_JOINT1", -m_jvel, 0.0); else m_my->setJointVelocity("RARM_JOINT1", m_jvel, 0.0); if(thetaR4<0) m_my->setJointVelocity("RARM_JOINT4", -m_jvel, 0.0); else m_my->setJointVelocity("RARM_JOINT4", m_jvel, 0.0); m_time_RA1 = DEG2RAD(abs(thetaR1) )/ m_jvel + evt.time(); m_time_RA4 = DEG2RAD(abs(thetaR4) )/ m_jvel + evt.time(); // ゴミを取りに関節を曲げる状態に移行します m_state = 12; } break; } // 関節を回転中 rotating arm for grasping case 12: { // 関節回転終了 if(evt.time() >= m_time_RA1) m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time_RA4) m_my->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time_RA1 && evt.time() >= m_time_RA4){ if(m_grasp) { //broadcastMsgToSrv("grasping the trash"); // 関節の回転を始める setting arm for taking double angR4 =m_my->getJointAngle("RARM_JOINT4")*180.0/(PI); double thetaR4 = -90.0-angR4; if(thetaR4<0) m_my->setJointVelocity("RARM_JOINT4", -m_jvel, 0.0); else m_my->setJointVelocity("RARM_JOINT4", m_jvel, 0.0); m_time_RA4 = DEG2RAD(abs(thetaR4) )/ m_jvel + evt.time(); // 関節を戻す状態に移行します m_state = 13; } else{ // graspできない broadcastMsgToSrv("Unreachable"); } } break; } // 関節を回転中 rotating arm for taking case 13: { // 関節回転終了 if(evt.time() >= m_time_RA4){ m_my->setJointVelocity("RARM_JOINT4", 0.0, 0.0); // 位置Aの方向に回転を開始します setting position a for rotating //broadcastMsgToSrv("Moving to the trashbox"); sendMsg("VoiceReco_Service","Now I will go to the trash boxes"); m_time = rotateTowardObj(pos_a, m_vel, evt.time()); m_state = 14; } break; } // 位置Aの方向に回転 rotating to position a case 14: { // 回転終了 if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // 位置Aに移動します setting position a for moving m_time = goToObj(pos_a, m_vel*4, 0.0, evt.time()); m_state = 15; } break; } // 位置Aの位置まで移動中 movig to position a case 15: { // 移動終了 if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); //broadcastMsgToSrv("Please tell me which trash box \n"); sendMsg("VoiceReco_Service","Please show me which trash box to use"); m_state = 16; } break; } // watig to point4 case 16: { break; } // ゴミ箱認識 starting trash box recognitiong-g-0 case 17: { // m_tpos object direction on object SimObj *target_trash = this->getObj(m_pointedtrash.c_str()); target_trash->getPosition(m_tpos); //broadcastMsgToSrv("Ok I will throw the trash in trash box \n"); msg_trash = "Ok I will put "+ m_pointedObject+"in"+ m_pointedtrash + "\n"; sendMsg("VoiceReco_Service",msg_trash); // ゴミの方向に回転をはじめる setting position trash box for rotating m_time = rotateTowardObj(m_tpos, m_vel, evt.time()); m_state = 18; break; } // ゴミ箱の方向に回転中 rotating to trash box case 18: { if(evt.time() >= m_time){ // 回転を止める m_my->setWheelVelocity(0.0, 0.0); //ゴミの位置まで移動をはじめる setting trash position for moving m_time = goToObj(m_tpos, m_vel*4, 30.0, evt.time()); m_state = 19; } break; } // ゴミを持ってゴミ箱に向かっている状態 moving to trash box case 19: { // ゴミ箱に到着 if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // grasp中のパーツを取得します getting grasped tarts CParts *parts = m_my->getParts("RARM_LINK7"); // releaseします parts->releaseObj(); // ゴミが捨てられるまで少し待つ sleep(1); // 捨てたゴミをゴミ候補から削除 deleting grasped object from list std::vector<std::string>::iterator it; it = std::find(m_trashes.begin(), m_trashes.end(), m_pointedObject); m_trashes.erase(it); // grasp終了 m_grasp = false; m_state = 1; } break; } } return 0.01; }
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::onAction(ActionEvent &evt) { if(first == false){ std::string msg = "start"; broadcastMsg(msg); } if(first==false){ FILE* fp; x=0; y=0; z=0; w=0; //チェックポイント if((fp = fopen("node2.txt", "r")) == NULL) { printf("File do not exist.\n"); exit(0); } while(fscanf(fp, "%lf,%lf,%lf,%lf", &x, &y, &z,&w) != EOF) { temp.x[i]=x; temp.y[i]=y; temp.z[i]=z; temp.w[i]=w; i++; } fclose(fp); first = true; i=0; } switch(m_state){ // 初期状態 case 0: { npos.x(temp.x[i]); npos.y(temp.y[i]); npos.z(temp.z[i]); m_time= rotateTowardObj(npos,m_vel,evt.time()); m_state = 1; break; } // 回転中 case 1: { // 回転終了 if(evt.time() >= m_time){ // 回転を止める m_my->setWheelVelocity(0.0, 0.0); m_time = goToObj(npos, m_vel*20, 1.0, evt.time()); m_state = 2; } break; } case 2: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); if(temp.w[i] == 1.0){ usleep(3200000); i++; }else if(temp.w[i] == 2.0){ std::string msg = "elevator"; //"man_000"にメッセージを送信します //broadcastMsgToSrv("Elevator"); sendMsg("man_000", msg); m_state = 0; i++; }else if(temp.w[i] == 3.0){ std::string msg = "ok"; //"man_000"にメッセージを送信します //broadcastMsgToSrv("Elevator"); sendMsg("man_000", msg); sleep(10); m_state = 0; i++; }else if(temp.w[i] == 4.0){ m_state =99; } else{ //std::string msg = "elevator"; //"man_000"にメッセージを送信します //broadcastMsgToSrv("Elevator"); //sendMsg("man_000", msg); m_state = 0; i++; } } break; } case 99: { if(flg==false){ m_my->setWheelVelocity(0.0, 0.0); //std::string msg = "Collision"; //broadcastMsg(msg); flg=true; } } } return 0.1; }
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::onAction(ActionEvent &evt) { /* if(!checkService("RecogTrash")){ m_srv == NULL; m_state = 0; return UPDATE_INTERVAL; } if(m_srv == NULL){ // ゴミ認識サービスが利用可能か調べる if(checkService("RecogTrash")){ // ゴミ認識サービスに接続 m_srv = connectToService("RecogTrash"); return UPDATE_INTERVAL; } }*/ //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_JOINT4", 0.0, 0.0); m_my->setJointVelocity("RARM_JOINT4", 0.0, 0.0); sendSceneInfo("Start"); printf("Started! \n"); m_executed = true; } 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()); if (m_lookObjFlg == 1.0) { printf("looking to Obj \n"); m_state = 810; } else { printf("go to next node \n"); m_state = 815; } 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 UPDATE_INTERVAL; }
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; }
void DemoRobotController::stopRobotMove(void) { m_robotObject->setWheelVelocity(0.0, 0.0); }
double MyController::onAction(ActionEvent &evt) { if(first == false){ std::string msg = "start"; broadcastMsg(msg); first = true; } int count=0; double r=0.0; //2点間の直線距離 double angle; if(end==false){ if(start==true){ Vector3d pos; Vector3d npos; if(first==false){ FILE* fp; x=0; y=0; z=0; w=0; //チェックポイント dx=0; dy=0; dz=0; if((fp = fopen("node.txt", "r")) == NULL) { printf("File do not exist.\n"); exit(0); } while(fscanf(fp, "%lf,%lf,%lf,%lf", &x, &y, &z,&w) != EOF) { temp.x[i]=x; temp.y[i]=y; temp.z[i]=z; temp.w[i]=w; i++; } fclose(fp); first = true; i=0; } if(stop==false){ my->getPosition(pos); npos.x(temp.x[i]); npos.y(temp.y[i]); npos.z(temp.z[i]); angle = rotateTowardObj(npos); if(angle < 0.0){ angle = -1.0 * angle; } /*ここに相当する部分を書く*/ //my->setAxisAndAngle(0, 1.0, 0, -angle); // 回転すべき円周距離 double distance = m_distance*PI*fabs(targetAngle)/(2*PI); if(targetAngle > 0.0){ m_my->setWheelVelocity(velocity, -velocity); } else{ m_my->setWheelVelocity(-velocity, velocity); } // 車輪の半径から移動速度を得る double vel = m_radius*velocity; // 回転時間(u秒) double time = (distance / vel) + evt.time(); if(evt.time>=time){ m_my->setWheelVelocity(0, 0); } dx=(temp.x[i]-pos.x()); dy=(temp.y[i]-pos.y()); dz=(temp.z[i]-pos.z()); r=sqrt(pow(dx,2)+pow(dz,2)); //ここまでが現在地から次の座標までの距離と角度の計算 double vel = m_radius*velocity; // 移動開始 m_my->setWheelVelocity(velocity, velocity); // 到着時間取得 double time2 = r / vel; count = 0; count2= 0; i++; } if(temp.w[i-1] == 1.0){ std::string msg = "point1"; //"robot_000"にメッセージを送信します sendMsg("man_001", msg); }else if(temp.w[i-1] == 2.0){ stop=true; broadcastMsgToSrv("elevator"); if(elevator==true){ stop=false; } } } } /***************************************************************************/ /* if(strstr(myname() , "robot_004")!= NULL){ m_my->setWheelVelocity(m_vel*2, m_vel*2); } */ return 0.1; }
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; } }
void MyController::onRecvMsg(RecvMsgEvent &evt) { // 送信者取得 std::string sender = evt.getSender(); std::cout << "sender: " << sender << std::endl; char *all_msg = (char*)evt.getMsg(); printf("all_msg: %s \n", all_msg); char *delim = (char *)(" "); char *ctx; char *header = strtok_r(all_msg, delim, &ctx); if (strcmp(header, "RESET") == 0) { printf("Received RESET \n"); setRobotPosition(0, -50); setRobotHeadingAngle(0); setCameraPosition(0, 3); printf("Reseted RobotPosition \n"); //char* replyMsg = sendSceneInfo(); sendSceneInfo("Start"); m_executed = true; return; } // 送信者がゴミ認識サービスの場合 if(sender == "RecogTrash") { /*char *all_msg = (char*)evt.getMsg(); //printf("all_msg: %s \n", all_msg); char *delim = (char *)(" "); char *ctx; char *header = strtok_r(all_msg, delim, &ctx);*/ double x = 0, y = 0, z = 0; // range = 0; if(strcmp(header, "ObjDir") == 0) { // 次に移動する座標を位置を取り出す x = atof(strtok_r(NULL, delim, &ctx)); y = atof(strtok_r(NULL, delim, &ctx)); z = atof(strtok_r(NULL, delim, &ctx)); m_range = atof(strtok_r(NULL, delim, &ctx)); nextPos.set(x, y, z); //printf("m_range = %lf \n", m_range); printf("[ClientMess] ObjDir %lf %lf %lf range: %lf\n", nextPos.x(), nextPos.y(), nextPos.z(), m_range); // ロボットのステートを更新 m_state = 20; m_executed = false; return; } // 物体のある場所に到着し、アームを伸ばし、物体を掴む if(strcmp(header, "grab") == 0) { printf("grab \n"); // 回転を止める m_my->setWheelVelocity(0.0, 0.0); m_state = 30; m_executed = false; return; } if(strcmp(header, "TrashBoxDir") == 0) { printf("TrashBoxDir \n"); // 次に移動する座標を位置を取り出す x = atof(strtok_r(NULL, delim, &ctx)); y = atof(strtok_r(NULL, delim, &ctx)); z = atof(strtok_r(NULL, delim, &ctx)); m_range = atof(strtok_r(NULL, delim, &ctx)); //printf("range = %lf \n", m_range); nextPos.set(x, y, z); printf("[ClientMess] TrashBoxDir %lf %lf %lf range: %lf\n", nextPos.x(), nextPos.y(), nextPos.z(), m_range); m_state = 40; m_executed = false; return; } if(strcmp(header, "ThrowTrash") == 0) { m_state = 50; m_executed = false; return; } if(strcmp(header, "Finish") == 0) { m_state = 100; m_executed = false; return; } if(strcmp(header, "RandomRouteStart") == 0) { m_state = 800; m_executed = false; return; } if(strcmp(header, "RandomRouteArrived") == 0) { //printf("onRecv RandomRouteArrived \n"); m_state = 800; m_executed = false; return; } if(strcmp(header, "RandomRoute") == 0) { x = atof(strtok_r(NULL, delim, &ctx)); z = atof(strtok_r(NULL, delim, &ctx)); m_range = atof(strtok_r(NULL, delim, &ctx)); nextPos.set(x, y, z); double lookingX = atof(strtok_r(NULL, delim, &ctx)); double lookingZ = atof(strtok_r(NULL, delim, &ctx)); m_lookingPos.set(lookingX, 0, lookingZ); m_state = 805; m_executed = false; return; } if(strcmp(header, "GoForwardVelocity") == 0) { double wheelVel = atof(strtok_r(NULL, delim, &ctx)); printf("GoForwardVelocity coef: %lf \n", wheelVel); wheelVel *= m_vel; m_my->setWheelVelocity(wheelVel * 10., wheelVel * 10.); //sendSceneInfo(); return; } if(strcmp(header, "RotateVelocity") == 0) { double wheelVel = atof(strtok_r(NULL, delim, &ctx)); printf("RotateVelocity coef: %lf \n", wheelVel); wheelVel *= m_vel; m_my->setWheelVelocity(-wheelVel, wheelVel); //sendSceneInfo(); return; } if(strcmp(header, "Stop") == 0) { printf("Stop joyStick \n"); m_my->setWheelVelocity(0.0, 0.0); sendSceneInfo(); return; } if (strcmp(header, "CamID") == 0) { printf("Setting CameraID \n"); m_CamID = atoi(strtok_r(NULL, delim, &ctx)); return; } if (strcmp(header, "CaptureData") == 0) { printf("SetRobotPosition \n"); double x = atof(strtok_r(NULL, delim, &ctx)); double z = atof(strtok_r(NULL, delim, &ctx)); setRobotPosition(x, z); double angle = atof(strtok_r(NULL, delim, &ctx)); printf("setRobotHeadingAngle: %lf \n", angle); setRobotHeadingAngle(angle); char* replyMsg = sendSceneInfo(); //m_srv->sendMsgToSrv(replyMsg); m_executed = true; return; } } else if (sender == "SIGViewer") { printf("mess from SIGViewer \n"); if(strcmp(header, "CameraAngle") == 0) { double angle = atof(strtok_r(NULL, delim, &ctx)); printf("CameraAngle: %lf \n", angle); setCameraPosition(angle, 3); char* replyMsg = sendSceneInfo(); m_executed = true; return; } else if (strcmp(header, "RobotAngle") == 0) { printf("rorate robor start \n"); double angle = atof(strtok_r(NULL, delim, &ctx)); printf("RobotHeadingAngle: %lf \n", angle); setRobotHeadingAngle(angle); char* replyMsg = sendSceneInfo(); //m_srv->sendMsgToSrv(replyMsg); m_executed = true; return; } else if (strcmp(header, "RotateDir") == 0) { // 次に移動する座標を位置を取り出す double x = atof(strtok_r(NULL, delim, &ctx)); double z = atof(strtok_r(NULL, delim, &ctx)); nextPos.set(x, 0, z); printf("RotateDir %lf %lf \n", nextPos.x(), nextPos.z()); // ロボットのステートを更新 m_state = 920; m_executed = false; return; } else if(strcmp(header, "RobotPosition") == 0) { double x = atof(strtok_r(NULL, delim, &ctx)); double z = atof(strtok_r(NULL, delim, &ctx)); setRobotPosition(x, z); return; } } }
void RobotController::stopRobotMove(void) { my->setWheelVelocity(0.0, 0.0); }
double MyController::onAction(ActionEvent &evt) { switch(m_state){ // 初期状態 case 0: { // ゴミがある場所と名前を取得します if(!this->recognizeTrash(m_tpos,m_tname)){ // ゴミが見つからないので終了 broadcastMsgToSrv("I cannot find trash"); m_state = 8; } // ゴミが見つかった else{ // ゴミの方向に回転をはじめる m_time = rotateTowardObj(m_tpos, m_vel, evt.time()); m_state = 1; } break; } // ゴミの方向に回転中 case 1: { // 回転終了 if(evt.time() >= m_time){ // 回転を止める m_my->setWheelVelocity(0.0, 0.0); // 関節の回転を始める // orig m_my->setJointVelocity("RARM_JOINT1", -m_jvel, 0.0); // 50°回転 m_time = DEG2RAD(50) / m_jvel + evt.time(); // ゴミを取りに関節を曲げる状態に移行します m_state = 2; } break; } // 関節を回転中 case 2: { // 関節回転終了 if(evt.time() >= m_time){ m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0); // graspしたいパーツを取得します CParts *parts = m_my->getParts("RARM_LINK7"); // graspします parts->graspObj(m_tname); // ゴミ箱の位置を取得します SimObj *trashbox = getObj("trashbox_1"); Vector3d pos; trashbox->getPosition(pos); // ゴミ箱の方向に移動を開始します m_time = rotateTowardObj(pos, m_vel, evt.time()); m_state = 3; } break; } // ゴミ箱の方向に回転中 case 3: { // ゴミ箱到着 if(evt.time() >= m_time){ // ここではゴミ箱の名前 位置は知っているものとします SimObj *trashbox = getObj("trashbox_1"); Vector3d pos; trashbox->getPosition(pos); // ゴミ箱の近くに移動します m_time = goToObj(pos, m_vel*4, 40.0, evt.time()); m_state = 4; } break; } // ゴミを持ってゴミ箱に向かっている状態 case 4: { // ゴミ箱に到着 if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // grasp中のパーツを取得します CParts *parts = m_my->getParts("RARM_LINK7"); // releaseします parts->releaseObj(); // ゴミが捨てられるまで少し待つ sleep(1); // 捨てたゴミをゴミ候補から削除 std::vector<std::string>::iterator it; it = std::find(m_trashes.begin(), m_trashes.end(), m_tname); m_trashes.erase(it); // 関節の回転を始める m_my->setJointVelocity("RARM_JOINT1", m_jvel, 0.0); m_time = DEG2RAD(50) / m_jvel + evt.time() + 1.0; m_state = 5; } break; } // ゴミを捨てて関節を戻している状態 case 5: { // 関節が元に戻った if(evt.time() >= m_time){ // 関節の回転を止める m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0); // 最初にいた方向に体を回転させます m_time = rotateTowardObj(m_inipos, m_vel, evt.time()); m_state = 6; } break; } // 元に場所に戻る方向に回転している状態 case 6: { if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // 最初にいた場所に移動します m_time = goToObj(m_inipos, m_vel*4, 5.0, evt.time()); m_state = 7; } break; } // 元の場所に向かっている状態 case 7: { // 元の場所に到着 if(evt.time() >= m_time){ m_my->setWheelVelocity(0.0, 0.0); // 最初の方向に回転(z軸と仮定) m_time = rotateTowardObj(Vector3d(0.0, 0.0, 10000.0), m_vel, evt.time()); m_state = 8; } break; } // 元の向きに回転している状態 case 8: { if(evt.time() >= m_time){ // 回転を止める m_my->setWheelVelocity(0.0, 0.0); // 最初の状態に戻る m_state = 0; } } } return 0.1; }
double RobotController::onAction(ActionEvent &evt) { //std::cout << "m_state " << m_state << std::endl; //std::cout << "the size of Vector " << Record_Postures.size() << std::endl; switch(m_state) { case 10: { Robot_speed = Change_Robot_speed; choose_task_arm(5, LEFT_ARM); choose_task_arm(5, RIGHT_ARM); // printf("got it in case!1 flag1 \n"); if (goTo(m_relayPoint1, 0) == true && moveArm(LEFT_ARM) == true && moveArm(RIGHT_ARM) == true) { m_state = 20; // printf("got it in case!1 \n"); } break; } case 20: { Robot_speed = Change_Robot_speed; if (goTo(m_relayPoint2, 0) == true) m_state = 30; break; } case 30: { Robot_speed = Change_Robot_speed; if (goTo(m_relayFrontTable, 0) == true) m_state = 40; break; } case 40: // Test if the cycle is finished or not { if (cycle > 0) { m_state = 41; broadcastMsg("Show_me"); get_Kinect_Data(); m_time = evt.time() + 5; break; } else { m_state = 49; break; } } case 41: { if(evt.time() > m_time) m_state = 42; break; } case 42: { Kinect_Data_Off(); // finished analysing data m_pointedObject = "petbottle"; m_pointedtrash = "recycle"; std::cout << "Task started Robot ........ " << std::endl; //PrintPosture(); m_state = 50; } case 49: { break; } case 50: //Optional case !!! { Robot_speed = Change_Robot_speed; if (m_pointedObject=="petbottle") { if (goTo(m_BottleFront, 0) == true) m_state = 60; } else if (m_pointedObject=="mugcup") { if (goTo(m_MuccupFront, 0) == true) m_state = 60; } else if (m_pointedObject=="can") { if (goTo(m_CanFront, 0) == true) m_state = 60; } break; } case 60: //preparation of the arm for grasp { Robot_speed = Change_Robot_speed; recognizeObjectPosition(m_Object_togo, m_pointedObject); if (goTo(m_Object_togo, 70) == true) { m_state = 70; } break; } case 70: //preparation of the arm for grasp { choose_task_arm(1, LEFT_ARM); if (moveArm(LEFT_ARM) == true) m_state = 80; break; } case 80: //move to the object { Robot_speed = 1; if (goTo(m_Object_togo, 38) == true) m_state = 90; break; } case 90: //move arm to grasp the object { choose_task_arm(2, LEFT_ARM); grasp_left_hand(); if (moveArm(LEFT_ARM) == true) m_state = 100; break; } case 100: { m_state = 110; break; } case 110: //move arm to place in good position for moving { grasp_left_hand(); choose_task_arm(3, LEFT_ARM); if (moveArm(LEFT_ARM) == true) m_state = 120; break; } case 120: { my->setWheelVelocity(-1.4,-1.4); choose_task_arm(5, LEFT_ARM); if (moveArm(LEFT_ARM) == true) { Robot_speed = Change_Robot_speed; m_state = 130; sleep(1); } break; } case 130: //move to the object { // Robot_speed = 1; if (goTo(m_relayFrontTrash, 0) == true) { m_state = 140; } break; } case 140: { if (goTo(m_relayFrontTable_reset, 0) == true) { sleep(2); m_state = 150; } break; } case 990: //move to the object { // Robot_speed = 1; break; } case 150: //Optional case !!! { Robot_speed = Change_Robot_speed; if (m_pointedtrash=="recycle") { if (goTo(m_RecycleFront, 0) == true) m_state = 160; } else if (m_pointedtrash=="burnable") { if (goTo(m_BurnableFront, 0) == true) m_state = 160; } else if (m_pointedtrash=="unburnable") { if (goTo(m_UnburnableFront, 0) == true) m_state = 160; } break; } case 160: //preparation of the arm for grasp { recognizeObjectPosition(m_Trash_togo, m_pointedtrash); if (goTo(m_Trash_togo, 50) == true) { m_state = 170; } break; } case 170: //preparation of the arm for grasp { choose_task_arm(1, LEFT_ARM); if (moveArm(LEFT_ARM) == true) m_state = 180; break; } case 180: { Robot_speed = 1; if (goTo(m_Trash_togo, 60) == true) m_state = 190; break; } case 190: //move arm to grasp the object { choose_task_arm(3, LEFT_ARM); if (moveArm(LEFT_ARM) == true) { m_state = 200; release_left_hand(); } break; } case 200: { my->setWheelVelocity(-2.5,-2.5); choose_task_arm(2, LEFT_ARM); if (moveArm(LEFT_ARM) == true) { Robot_speed = Change_Robot_speed; m_state = 210; } break; } case 210: //move to the object { Robot_speed = Change_Robot_speed; if (goTo(m_relayFrontTrash, 0) == true) m_state = 220; break; } case 220: //preparation of the arm for grasp { choose_task_arm(5, LEFT_ARM); if (moveArm(LEFT_ARM) == true) m_state = 230; break; } case 230: //move to the object { Robot_speed = Change_Robot_speed; if (goTo(m_relayFrontTable, 0) == true) { cycle = cycle-1; m_state = 40; broadcastMsg("Task_finished"); m_pointedtrash = ""; m_pointedObject = ""; m_state = 0; } break; } } return m_onActionReturn; }
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 DemoRobotController::onAction(ActionEvent &evt) { switch(m_state) { case 0: { break; } case 1: { this->stopRobotMove(); break; } case 50: { // detour: rotate toward relay point 1 if(evt.time() >= m_time) { this->stopRobotMove(); double l_moveTime = rotateTowardObj(m_relayPoint1); m_time = l_moveTime+evt.time(); m_state = 60; } break; } case 60: { // detour: go toward relay point 1 if(evt.time() >= m_time) { this->stopRobotMove(); double l_moveTime = goToObj(m_relayPoint1, 0.0); m_time = l_moveTime+evt.time(); m_state = 70; } break; } case 70: { // rotate toward the position in front of trash if(evt.time() >= m_time) { this->stopRobotMove(); double l_moveTime = rotateTowardObj(m_frontDesk1); m_time = l_moveTime+evt.time(); m_state = 80; } break; } case 80: { // go toward the position in front of trash if(evt.time() >= m_time) { this->stopRobotMove(); double l_moveTime = goToObj(m_frontDesk1, 0.0); m_time = l_moveTime+evt.time(); m_state = 90; } break; } case 90: { // rotate toward the trash if(evt.time() >= m_time) { this->stopRobotMove(); Vector3d l_tpos; if(m_task == 1) this->recognizeObjectPosition(l_tpos, m_trashName1); else this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime+evt.time(); m_state = 100; } break; } case 100: { // prepare the robot arm to grasping the trash if(evt.time() >= m_time) { this->stopRobotMove(); this->neutralizeArms(evt.time()); m_state = 105; } break; } case 105: { // fix robot direction for grasping if(evt.time() >= m_time1) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4) { Vector3d l_tpos; if(m_task == 1) this->recognizeObjectPosition(l_tpos, m_trashName1); else this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime+evt.time(); m_state = 110; } break; } case 110: { // approach to the trash if(evt.time() >= m_time) { this->stopRobotMove(); Vector3d l_tpos; if(m_task == 1) this->recognizeObjectPosition(l_tpos, m_trashName1); else this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = goToObj(l_tpos, 30.0); m_time = l_moveTime+evt.time(); m_state = 120; } break; } case 120: { // try to grasp trash if(evt.time() >= m_time) { this->stopRobotMove(); Vector3d l_tpos; if(m_task == 1) this->recognizeObjectPosition(l_tpos, m_trashName1); else this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = goGraspingObject(l_tpos); m_time = l_moveTime+evt.time(); m_state = 125; } break; } case 125: { if(evt.time() >= m_time) { m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); this->neutralizeArms(evt.time()); m_state = 130; } break; } case 130: { if(evt.time() >= m_time1) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4) { m_robotObject->setWheelVelocity(-m_angularVelocity, -m_angularVelocity); m_time = 20./m_movingSpeed + evt.time(); m_state = 150; } break; } case 150: { if(evt.time() >= m_time) { this->stopRobotMove(); double l_moveTime; if(m_task == 1) l_moveTime = rotateTowardObj(m_frontTrashBox1); else l_moveTime = rotateTowardObj(m_frontTrashBox2); m_time = l_moveTime + evt.time(); m_state = 160; } break; } case 160: { if(evt.time() >= m_time) { this->stopRobotMove(); double l_moveTime; if(m_task == 1) l_moveTime = goToObj(m_frontTrashBox1,0.0); else l_moveTime = goToObj(m_frontTrashBox2,0.0); m_time = l_moveTime + evt.time(); m_state = 161; } break; } case 161: { if(evt.time() >= m_time) { this->stopRobotMove(); this->prepareThrowing(evt.time()); m_state = 165; } break; } case 165: { if(evt.time() >= m_time1) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4) { Vector3d l_tpos; if(m_task == 1) this->recognizeObjectPosition(l_tpos, m_trashBoxName1); else this->recognizeObjectPosition(l_tpos, m_trashBoxName2); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime + evt.time(); m_state = 170; } break; } case 170: { if(evt.time() >= m_time) { this->stopRobotMove(); Vector3d l_tpos; if(m_task == 1) this->recognizeObjectPosition(l_tpos, m_trashBoxName1); else this->recognizeObjectPosition(l_tpos, m_trashBoxName2); double l_moveTime = goToObj(l_tpos, 50.0); m_time = l_moveTime + evt.time(); m_state = 180; } break; } case 180: { if(evt.time() >= m_time) { this->stopRobotMove(); Vector3d l_tpos; if(m_task == 1) this->recognizeObjectPosition(l_tpos, m_trashBoxName1); else this->recognizeObjectPosition(l_tpos, m_trashBoxName2); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime + evt.time(); m_state = 200; } break; } case 200: { // throw trash and get back a bit if(evt.time() >= m_time) { this->stopRobotMove(); this->throwTrash(); sleep(1); m_robotObject->setWheelVelocity(-m_angularVelocity, -m_angularVelocity); m_time = 50.0/m_movingSpeed + evt.time(); m_state = 225; } break; } case 225: { // recover robot arms if(evt.time() >= m_time) { this->stopRobotMove(); this->neutralizeArms(evt.time()); m_state = 240; } break; } case 240: { // go next if(evt.time() >= m_time1) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4) { this->stopRobotMove(); broadcastMsg("Task_finished"); //broadcastMsg("Give_up"); m_state = 0; } break; } } return refreshRateOnAction; }