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 MyController::calcHeadingAngle() { // 自分の回転を得る 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; } return theta * 180.0 / PI; }
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::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::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); 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; } }
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; }