// object まで移動 double RobotController::goToObj(Vector3d pos, double velocity, double range, double now) { Vector3d myPos; //m_my->getPosition(myPos); m_my->getPartsPosition(myPos,"RARM_LINK2"); // 自分の位置からターゲットを結ぶベクトル 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; }
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; }
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; } }
double RobotController::rotateTowardObj(Vector3d pos, double velocity, double now) { // 自分の位置の取得 Vector3d myPos; //m_my->getPosition(myPos); m_my->getPartsPosition(myPos,"RARM_LINK2"); // 自分の位置からターゲットを結ぶベクトル 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 RobotController::onInit(InitEvent &evt) { m_robotState = 0; //エージェントの正面の定義はz軸の正の向きを向いていると仮定する m_defaultNormalVector = Vector3d(0,0,1); m_onActionReturn = 1.0; m_speedDelta = 2.0; m_avatar = "man_000"; m_my = getRobotObj(myname()); // 初期位置取得 //m_my->getPosition(m_inipos); m_my->getPartsPosition(m_inipos,"RARM_LINK2"); pos_a = Vector3d(-100, 30, -110);// pos_b = Vector3d(0, 30, -110); /// // 車輪間距離 m_distance = 10.0; // 車輪半径 m_radius = 10.0; // 移動終了時間初期化 m_time = 0.0; m_time_LA1 = 0.0; m_time_LA4 = 0.0; m_time_RA1 = 0.0; m_time_RA4 = 0.0; // 車輪の半径と車輪間距離設定 m_my->setWheel(m_radius, m_distance); m_state = 20; srand((unsigned)time( NULL )); // 車輪の回転速度 m_vel = 0.3; // 関節の回転速度 m_jvel = 0.6; // grasp初期化 m_grasp = false; m_recogSrv = NULL; m_sended = false; // ここではゴミの名前が分かっているとします //m_trashes.push_back("petbottle_0"); //m_trashes.push_back("petbottle_1"); //m_trashes.push_back("petbottle_2"); //m_trashes.push_back("petbottle_3"); m_trashes.push_back("petbottle"); //m_trashes.push_back("banana"); //m_trashes.push_back("chigarette"); //m_trashes.push_back("chocolate"); //m_trashes.push_back("mayonaise_0"); //m_trashes.push_back("mayonaise_1"); m_trashes.push_back("mugcup"); //m_trashes.push_back("can_0"); //m_trashes.push_back("can_1"); m_trashes.push_back("can"); //m_trashes.push_back("can_3"); // ゴミ箱登録 m_trashboxs.push_back("bluetrashbox"); m_trashboxs.push_back("redtrashbox"); m_trashboxs.push_back("greentrashbox"); m_trashboxs.push_back("wagon"); }
void RobotController::onInit(InitEvent &evt) { m_robotState = 0; //エージェントの正面の定義はz軸の正の向きを向いていると仮定する m_defaultNormalVector = Vector3d(0,0,1); m_onActionReturn = 0.01; m_speedDelta = 2.0; m_my = getRobotObj(myname()); // 初期位置取得 m_my->getPartsPosition(m_inipos,"RARM_LINK2"); m_relayPoint0 = Vector3d(0, 30, 0); m_relayPoint1 = Vector3d(0, 30, 350); m_relayPoint2 = Vector3d(-400, 30, 0); m_kitchenPoint = Vector3d(200, 30, 350);; m_bedroomPoint = Vector3d(-400, 30, 200);; m_lobbyPoint = Vector3d(-400, 30, -200);; // 車輪間距離 m_distance = 10.0; // 車輪半径 m_radius = 10.0; // 移動終了時間初期化 m_time = 0.0; m_time_LA1 = 0.0; m_time_LA4 = 0.0; m_time_RA1 = 0.0; m_time_RA4 = 0.0; // 車輪の半径と車輪間距離設定 m_my->setWheel(m_radius, m_distance); m_state = 0; srand((unsigned)time( NULL )); // 車輪の回転速度 m_vel = 1.0; // 関節の回転速度 m_jvel = 1.0; // grasp初期化 m_grasp = false; m_recogSrv = NULL; m_sended = false; // ここではゴミの名前が分かっているとします m_trashes.push_back("petbottle_1"); m_trashes.push_back("can_0"); m_trashes.push_back("apple"); // ゴミ箱登録 m_trashboxs.push_back("trashbox_0"); m_trashboxs.push_back("trashbox_1"); m_trashboxs.push_back("trashbox_2"); broadcastMsgToSrv("Waiting 'start' message\n"); }