// 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");
}
