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