double MyController::onAction(ActionEvent &evt)
{
robotObj = getObj(robotName.c_str());
operatorObj = getObj(operatorName.c_str());
Vector3d robotPos;
robotObj->getPosition(robotPos);
// 位置取得
Vector3d operatorPos;
operatorObj->getPosition(operatorPos);
Vector3d vec(operatorPos.x()-robotPos.x(), operatorPos.y()-robotPos.y(), operatorPos.z()-robotPos.z());
double distance = sqrt((vec.x()*vec.x())+(vec.z()*vec.z()));
if(distance>=WAIT_DISTANCE){
if(follow){
std::string msg = "NotFollowing";
sendMsg(operatorName.c_str(),msg);
follow = false;
//LOG_MSG((msg.c_str()));
}
}
else{
if(!follow){
std::string msg = "Following";
sendMsg(operatorName.c_str(),msg);
follow = true;
//LOG_MSG((msg.c_str()));
}
}
return 0.1;
}
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;
}
double MyController::onAction(ActionEvent &evt) {
int actionNumber = 5;
int functionalFeature = 3;
myfile << setprecision(2) << std::fixed;
SimObj *target = getObj("box_015");
SimObj *toolName = getObj("TShapeTool_015");
maintainOrientationOfTool(toolName, initToolRotation);
isToolAtRest = checkEntityMotionStatus(toolName); // checks whether the tool is moving by calculating its velocity
isTargetAtRest = checkEntityMotionStatus(target); // checks whether the object is moving by calculating its velocity
if(isToolAtRest)
{
toolName->getPosition(currentToolPos);
}
if (isTargetAtRest && isToolAtRest)
{
target->getPosition(currentTargetPos);
}
if (isTargetAtRest && isToolAtRest && flag )
{
myfile << actionNumber << " , " << functionalFeature << " , " ;
myfile << forceOnTool_x << " , " << forceOnTool_z << " , " ;
myfile << initToolPos.x() << " , " << initToolPos.z() << " , " ;
myfile << initTargetPos.x() << " , " << initTargetPos.z() << " , " ;
myfile << currentTargetPos.x() << " , " << currentTargetPos.z() << " , " ;
myfile << currentTargetPos.x() - initTargetPos.x() << " , " << currentTargetPos.z() - initTargetPos.z();
myfile << "\n";
cout << "The simulation for " << actionNumber << " , " << functionalFeature << " has been recorded" << endl;
// exit(0);
flag = false;
}
return 0.01;
}
void CameraController::rotateTowardRobot(Vector3d r_pos)
{
m_my->setPosition(m_rotatePos);
// カメラの位置を得る
m_my->getPosition(m_pos);
// カメラの位置からロボットを結ぶベクトル
Vector3d tmpp = r_pos;
tmpp -= m_pos;
// y方向は考えない
tmpp.y(0);
// カメラの回転角度を得る
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;
// ロボットまでの回転角度を得る
double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
double targetAngle = acos(tmp);
if(tmpp.x() > 0) targetAngle = -1*targetAngle;
// 角度差から回転量を得る
targetAngle += theta;
m_my->setAxisAndAngle(0, 1, 0, -targetAngle, 0);
}
void MyController::onInit(InitEvent &evt) {
// handle of target and tool
SimObj *toolName = getObj("TShapeTool_7");
SimObj *target = getObj("box_7");
toolName->setMass(10.0); // mass of all the tools should be set uniformly
target->getPosition(initTargetPos); // initial target position
target->getRotation(initTargetRotation); // initial target rotation in quaternion
toolName->getPosition(initToolPos); // initial tool position
toolName->getRotation(initToolRotation); // initial tool rotation in quaternion
isTargetAtRest = true; // target and tool both are at rest initially.
isToolAtRest = true;
insideTimer = true;
f_x = 0 ;
f_z = 0 ;
int xForceVariance = 4000;
int zForceVariance = 4000;
flag = true;
Colli = false;
counterOfCollision = 0;
counterOfAction = 0;
// Reset the forces applied
double r;
r = ((double) rand() / (RAND_MAX)) ;
f_x = r * int(xForceVariance);
f_z = r * int(zForceVariance);
forceOnTool_z = 8000 + f_z;
forceOnTool_x = 0;
forceOnTool.set(forceOnTool_x, 0 , forceOnTool_z);
toolName->addForce(forceOnTool.x(), forceOnTool.y(), forceOnTool.z());
toolName->getVelocity(appliedToolVel);
myfile.flush(); // I have uncommented the file, because I want to overwrite the file.
}
void DemoRobotController::recognizeObjectPosition(Vector3d &pos, std::string &name)
{
// get object of trash selected
SimObj *trash = getObj(name.c_str());
// get trash's position
trash->getPosition(pos);
}
void MyController::onInit(InitEvent &evt) {
SimObj *stick = getObj("robot_test");
// stick->addForce(-5000,0,5000);
stick->getPosition(startPosition);
// stick->getRotation(initialToolRot);
}
void MyController::confirmThrewTrashPos(Vector3d &pos, std::string &name) {
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());
} else {
//printf("cannot find trashbox with such name \n");
}
return;
}
double MyController::onAction(ActionEvent &evt)
{
int count=0;
Vector3d pos;
if(start==true){
while(count<20){
if(sw == false){
my->getPosition(pos);
my->setPosition(pos.x(),pos.y(),pos.z()-10);
my->setJointAngle("LARM_JOINT1", DEG2RAD(-30));
my->setJointAngle("RLEG_JOINT2", DEG2RAD(-20));
my->setJointAngle("RLEG_JOINT4", DEG2RAD(10));
usleep(100000);
my->setJointAngle("LARM_JOINT1", DEG2RAD(0));
my->setJointAngle("RLEG_JOINT2", DEG2RAD(0));
my->setJointAngle("RLEG_JOINT4", DEG2RAD(0));
sw = true;
}else{
my->getPosition(pos);
my->setPosition(pos.x(),pos.y(),pos.z()-10);
my->setJointAngle("RARM_JOINT1", DEG2RAD(-30));
my->setJointAngle("LLEG_JOINT2", DEG2RAD(-20));
my->setJointAngle("LLEG_JOINT4", DEG2RAD(10));
usleep(100000);
my->setJointAngle("RARM_JOINT1", DEG2RAD(0));
my->setJointAngle("LLEG_JOINT2", DEG2RAD(0));
my->setJointAngle("LLEG_JOINT4", DEG2RAD(0));
sw = false;
}
count++;
start=false;
}
}
return 0.1;
}
std::string RobotController::getPointedTrashName(std::string entName)
{
// 発話者の名前からSimObjを取得します
SimObj *tobj = getObj(entName.c_str());
// メッセージ送信者の左肘関節の位置を取得します
Vector3d jpos;
if(!tobj->getJointPosition(jpos, "RARM_JOINT4")) {
LOG_ERR(("failed to get joint position"));
return "";
}
// メッセージ送信者の左肘から左手首をつなぐベクトルを取得します
Vector3d jvec;
if(!tobj->getPointingVector(jvec, "RARM_JOINT4", "RARM_JOINT7")) {
LOG_ERR(("failed to get pointing vector"));
return "";
}
double distance = 0.0;
std::string objName = "";
// 全ゴミオブジェクトでループします
int trashboxSize = m_trashboxs.size();
for(int i = 0; i < trashboxSize; i++) {
// エンティティの位置を取得します
SimObj *obj = getObj(m_trashboxs[i].c_str());
Vector3d objVec;
obj->getPosition(objVec);
// エンティティと左肘関節を結ぶベクトルを作成します
objVec -= jpos;
// cos角度が不の場合（指差した方向と反対側にある場合)は対象から外します
double cos = jvec.angle(objVec);
if(cos < 0)
continue;
// 指差した方向ベクトルまでの最短距離の計算
double theta = acos(cos);
double tmp_distance = sin(theta) * objVec.length();
// 最小距離の場合は名前、距離を保存しておく
if(tmp_distance < distance || distance == 0.0){
distance = tmp_distance;
objName = obj->name();
}
}
// エンティティでループして最も近いオブジェクトの名前を取得する
return objName;
}
void MyController::onInit(InitEvent &evt) {
m_my = getObj(myname());
// この範囲で判定
checkSize_x = 40.0;
checkSize_z = 80.0;
// 自分の位置取得
m_my->getPosition(myPos);
sentMsg = false;
flag1 = false;
flag2 = false;
}
bool RobotController::recognizeTrash(Vector3d &pos, std::string &name)
{
// 候補のゴミが無い場合
if(m_trashes.empty()){
return false;
}
// ここでは乱数を使ってゴミを決定します
int trashNum = rand() % m_trashes.size();
// ゴミの名前と位置を取得します
name = m_trashes[trashNum];
SimObj *trash = getObj(name.c_str());
// ゴミの位置取得
trash->getPosition(pos);
return true;
}
void RobotController::grasp_left_hand()
{
Vector3d hand, object;
SimObj *obj = getObj(m_pointedObject.c_str());
obj->getPosition(object);
my->getJointPosition(hand, "LARM_JOINT7");
double distance = getDist3D(hand,object);
if (distance < GRASPABLE_DISTANCE && m_grasp_left == false)
{
CParts * parts = my->getParts("LARM_LINK7");
if (parts->graspObj(m_pointedObject))
{
m_grasp_left = true;
// broadcastMsg("Object_grasped");
}
}
}
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();
}
/* ゴミの名前から、捨てるべきゴミ箱の名前を検索し、そのゴミ箱の位置を探す
* @return pos 捨てるべきの位置
* @param trashName ゴミの名前
*/
bool MyController::findPlace2PutObj(Vector3d &pos, std::string trashName)
{
/////////////////////////////////////////////
///////////ここでゴミを認識します////////////
/////////////////////////////////////////////
// 候補のゴミが無い場合
if(m_trashBoxes.empty()){
return false;
} else {
//printf("m_trashBoxes.size(): %d \n", m_trashBoxes.size());
}
std::string trashBoxName = "";
std::map<std::string, std::string>::iterator it = m_trashTypeMap.begin();
trashBoxName = m_trashTypeMap.find(trashName)->second;
std::cout << trashName << " => " << trashBoxName << '\n';
bool trashBoxExist = false;
for(int i = 0; i < m_trashBoxes.size(); i++) {
if(m_trashBoxes[i] == trashBoxName) {
trashBoxExist = true;
}
}
if(trashBoxExist) {
if(getObj(trashBoxName.c_str())) {
SimObj *place = getObj(trashBoxName.c_str());
// ゴミの位置取得
place->getPosition(pos);
//printf("ゴミ箱の位置： %lf %lf %lf \n", pos.x(), pos.y(), pos.z());
return true;
} else {
printf("can not get obj with such name \n");
}
} else {
return false;
}
return false;
}
void MyController::onInit(InitEvent &evt)
{
my = getObj(myname());
my->getPosition(initPos);
start = false;
sw = false;
// 手を下げる
my->setJointAngle("LARM_JOINT2", DEG2RAD(-90));
my->setJointAngle("RARM_JOINT2", DEG2RAD(90));
stepWidth = 70;
sleeptime = 300000;
if((fp = fopen("motion.txt", "r")) == NULL) {
LOG_MSG(("File do not exist."));
}
else{
fscanf(fp, "%d", &motionNum);
fscanf(fp, "%d", &sleeptime);
for(int i=0; i<motionNum; i++){
fscanf(fp, "%f %f %f %f %f %f %f %f %f %f %f",
&HEIGHT[i],
&LARM_JOINT1[i],
&LARM_JOINT3[i],
&RARM_JOINT1[i],
&RARM_JOINT3[i],
&LLEG_JOINT2[i],
&LLEG_JOINT4[i],
&LLEG_JOINT6[i],
&RLEG_JOINT2[i],
&RLEG_JOINT4[i],
&RLEG_JOINT6[i]);
}
}
count = 0;
step = 6;
}
double AgentController::onAction(ActionEvent &evt)
{
try {
static int deg = 0;
SimObj *my = getObj(myname());
Vector3d v;
my->getPosition(v);
LOG_MSG(("pos = (%f, %f, %f)", v.x(), v.y(), v.z()));
//my->setJointAngle("R_SHOULDER", DEG2RAD(deg));
my->setJointAngle("R_ELBOW", DEG2RAD(90));
my->setJointAngle("R_SHOULDER", DEG2RAD(deg));
//my->setJointAngle("R_SHOULDER", DEG2RAD(deg));
Parts *p = my->getParts("RU_ARM");
if (p) {
const double *pos = p->getPosition();;
LOG_MSG(("RU_ARM(%f, %f, %f)", pos[0], pos[1], pos[2]));
const double *q = p->getQuaternion();
LOG_MSG((" (%f, %f, %f, %f", q[0], q[1], q[2], q[3]));
}
p = my->getParts("RL_ARM");
if (p) {
const double *pos = p->getPosition();;
LOG_MSG(("RL_ARM(%f, %f, %f)", pos[0], pos[1], pos[2]));
const double *q = p->getQuaternion();
LOG_MSG((" (%f, %f, %f, %f", q[0], q[1], q[2], q[3]));
}
deg += 45;
} catch(SimObj::Exception &) {
;
}
return 0.1;
}
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;
}
void MyController::onInit(InitEvent &evt) {
SimObj *target = getObj("box_015");
SimObj *toolName = getObj("TShapeTool_015");
toolName->setMass(10.0);
target->getPosition(initTargetPos);
target->getRotation(initTargetRotation);
toolName->getPosition(initToolPos);
toolName->getRotation(initToolRotation);
isTargetAtRest = true;
isToolAtRest = true;
f_x = 0 ;
f_z = 0 ;
int xForceVariance = 2000;
int zForceVariance = 2000;
// Reset the forces applied
double r;
r = ((double) rand() / (RAND_MAX)) ;
f_x = r * int(xForceVariance);
f_z = r * int(zForceVariance);
flag = true;
Colli = false;
counter = 0;
forceOnTool_x = 5000 + f_z;
forceOnTool_z = 5000 + f_z;
// cout << "The applied force " << forceOnTool_x << " , " << forceOnTool_z << endl;
forceOnTool.set(forceOnTool_x, 0 , forceOnTool_z);
tapWithTool(toolName, initToolRotation, forceOnTool);
myfile.flush(); // I have uncommented the file, because I want to overwrite the file.
// if (myfile.is_open())
// {
// myfile << "Action" << " , " << "FunctionalFeature" << " , " ;
// myfile << "forceOnTool_X" << " , " << "forceOnTool_Z " << " , ";
// myfile << "toolVelAtHit_X" << " , " << "toolVelAtHit_Z" << " , ";
// myfile << "InitialToolPos_X" << " , " << "InitialToolPos_Z" << " , ";
// myfile << "InitialTargetPos_X" << " , " << "InitialTargetPos_Z" << " , ";
// myfile << "targetFinalPos_X" << " , " << "targetFinalPos_Z" << " , ";
// myfile << "targetDisplacement_X" << " , " << "targetDisplacement_Z";
// myfile << "\n" ;
// }
}
double MyController::onAction(ActionEvent &evt) {
int actionNumber = 2;
int functionalFeature = 1;
int targetType = 3;
myfile << setprecision(2) << std::fixed;
// handle of target and tool
SimObj *target = getObj("box_004");
SimObj *toolName = getObj("TShapeTool_004");
if (evt.time() < 5.0)
{
// cout << "Time" << endl;
cout << evt.time() << endl;
toolName->getPosition(currentToolPos); // get the current tool position
toolName->getRotation(finalToolRotation);
toolName->getVelocity(finalToolVel);
isToolAtRest = checkEntityMotionStatus(toolName); // checks whether the tool is moving by calculating its velocity
target->getPosition(currentTargetPos);
target->getRotation(finalTargetRotation);
target->getVelocity(finalTargetVel);
isTargetAtRest = checkEntityMotionStatus(target); // checks whether the object is moving by calculating its velocity
}
if (evt.time() > 5.0)
{
insideTimer = false;
counterOfAction ++ ;
}
if(!insideTimer && counterOfAction == 1 )
{
myfile << actionNumber << " , " << functionalFeature << " , " ;
myfile << initToolRotation.qw() << " , " << initToolRotation.qx() << " , " << initToolRotation.qy() << " , " << initToolRotation.qz() << " , " ;
myfile << initTargetRotation.qw() << " , " << initTargetRotation.qx() << " , " << initTargetRotation.qy() << " , " << initTargetRotation.qz() << " , " ;
myfile << finalTargetRotation.qw() << " , " << finalTargetRotation.qx() << " , " << finalToolRotation.qy() << " , " << finalToolRotation.qz() << " , " ;
myfile << initToolPos.x() << " , " << initToolPos.z() << " , " ;
myfile << initTargetPos.x() << " , " << initTargetPos.z() << " , " ;
myfile << forceOnTool_x << " , " << forceOnTool_z << " , " ;
myfile << appliedToolVel.x() << " , " << appliedToolVel.z() << " , " ;
myfile << toolVelAtHit.x() << " , " << toolVelAtHit.z() << " , " ;
myfile << targetVelAtHit.x() << " , " << targetVelAtHit.z() << " , " ;
myfile << currentToolPos.x() << " , " << currentToolPos.z() << " , " ;
myfile << currentTargetPos.x() << " , " << currentTargetPos.z() << " , " ;
myfile << finalToolVel.x() << " , " << finalToolVel.z() << " , " ;
myfile << finalTargetVel.x() << " , " << finalTargetVel.z() << " , " ;
myfile << isToolAtRest << " , " << isTargetAtRest << " , " ;
myfile << currentToolPos.x() - initToolPos.x() << " , " << currentToolPos.z() - initToolPos.z() << " , " ;
myfile << currentTargetPos.x() - initTargetPos.x() << " , " << currentTargetPos.z() - initTargetPos.z();
myfile << "\n";
cout << "The simulation for " << actionNumber << " , " << functionalFeature << " has been recorded" << endl;
// exit(0);
flag = false;
}
return 0.01;
}
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)
{
// サービスが使用可能か定期的にチェックする
bool available = checkService("RobocupReferee");
if(!available && m_ref != NULL) m_ref = NULL;
// 使用可能
else if(available && m_ref == NULL){
// サービスに接続
m_ref = connectToService("RobocupReferee");
}
// 自分の位置取得
Vector3d myPos;
m_my->getPosition(myPos);
// 衝突中の場合,衝突が継続しているかチェック
if(colState){
CParts *parts = m_my->getMainParts();
bool state = parts->getCollisionState();
// 衝突していない状態に戻す
if(!state) colState = false;
}
int entSize = m_entities.size();
for(int i = 0; i < entSize; i++){
// ロボットまたはゴミ箱の場合は除く
if(m_entities[i] == "robot_000" ||
m_entities[i] == "recycle" ||
m_entities[i] == "burnable" ||
m_entities[i] == "room" ||
m_entities[i] == "moderator_0" ||
m_entities[i] == "Kinect_000" ||
m_entities[i] == "unburnable"){
continue;
}
// エンティティ取得
SimObj *ent = getObj(m_entities[i].c_str());
// 位置取得
Vector3d tpos;
ent->getPosition(tpos);
// ゴミ箱からゴミを結ぶベクトル
Vector3d vec(tpos.x()-myPos.x(), tpos.y()-myPos.y(), tpos.z()-myPos.z());
// ゴミがゴミ箱の中に入ったかどうか判定
if(abs(vec.x()) < tboxSize_x/2.0 &&
abs(vec.z()) < tboxSize_z/2.0 &&
tpos.y() < tboxMax_y &&
tpos.y() > tboxMin_y ){
// ゴミがリリースされているか確認
if(!ent->getIsGrasped()){
// ゴミを捨てる
tpos.y(-100);
tpos.x(myPos.x());
tpos.z(myPos.z());
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
ent->setPosition(tpos);
ent->setPosition(tpos);
usleep(500000);
tpos.y(-50.0);
ent->setPosition(tpos);
ent->setPosition(tpos);
ent->setPosition(tpos);
std::string msg;
// ゴミが所定のゴミ箱に捨てられているかチェック
// リサイクル
/*
if(strcmp(myname(), "recycle") == 0){
// 空のペットボトルのみ点が入る
if(strcmp(ent->name(), "petbottle") == 0 ||
strcmp(ent->name(), "petbottle_2") == 0 ||
strcmp(ent->name(), "petbottle_4") == 0 ||
strcmp(ent->name(), "mayonaise_1") == 0 ) {
msg = "RobocupReferee/Clean up succeeded" "/1000";
}
else{
msg = "RobocupReferee/Clean up failed" "/-600";
}
}
*/
// 燃えるゴミ
/*
else if(strcmp(myname(), "burnable") == 0){
// 燃えるゴミに入れるべきものは無い
msg = "RobocupReferee/Clean up failed" "/-600";
}
// 缶瓶
else if(strcmp(myname(), "unburnable") == 0){
if(strcmp(ent->name(), "can_0") == 0 ||
strcmp(ent->name(), "can_1") == 0 ||
strcmp(ent->name(), "can") == 0 ||
strcmp(ent->name(), "can_3") == 0) {
msg = "RobocupReferee/Clean up succeeded" "/1000";
}
else {
msg = "RobocupReferee/Clean up succeeded" "/-600";
}
}
*/
if(m_ref != NULL) {
// m_ref->sendMsgToSrv(msg.c_str());
}
//LOG_MSG((msg.c_str()));
}
}
}
return retValue;
}
double MyController::onAction(ActionEvent &evt) {
dlopen("libpython2.7.so", RTLD_LAZY | RTLD_GLOBAL);
Py_Initialize(); //initialization of the python interpreter
//return 1.0;
SimObj *stick = getObj("robot_test");
SimObj *box = getObj("box_001");
SimObj *goal_001 = getObj("checkpoint_001");
double torque;
// The target position
Vector3d targetPos;
box->getPosition(targetPos);
// The Goal Position: The sphere is placed at Goal position.
Vector3d goalPos;
goal_001->getPosition(goalPos);
// calculating the displacement vector
Vector3d displacementVect;
displacementVect.x(goalPos.x()-targetPos.x());
displacementVect.y(goalPos.y()-targetPos.y());
displacementVect.z(goalPos.z()-targetPos.z());
// stick->addForce(-500,0,500); // This adds force to on the stick tool.
// stick->addForce(0,0,5000);
// Vector3d angularVel;
// stick->getAngularVelocity(angularVel);
// LOG_MSG((" Current angular Velocity is : %f %f %f ", angularVel.x(), angularVel.y(), angularVel.z() ));
// if ( abs(angularVel.y()) > 0.1 )
// {
// LOG_MSG((" Current angular Velocity is : %f %f %f ", angularVel.x(), angularVel.y(), angularVel.z() ));
// double* ptr1 = NULL;
// ptr1 = controlAngularVelocity(angularVel, 3.0, 0, 1.0);
// torque = ptr1[0] * 500;
// // torque = ptr1[0] * 12000 ;
// cout << "The torque applied for controlling angular velocity is " << torque << " N. m" << endl;
// stick->addTorque( 0 , torque, 0);
// }
// to control the rotation of the tool
// Rotation currentToolRot;
// stick->getRotation(currentToolRot);
// double *ptr = NULL;
// ptr = controlRotation(initialToolRot, currentToolRot, 20.0, 0.0, 20.0);
// torque = ptr[1] * 200 ;
// cout << "The torque applied for controlling the rotation = " << torque << endl;
// stick->addTorque(0, torque,0);
// double massOfTool;
// massOfTool = stick->getMass();
// cout << "The mass is " << massOfTool <<endl;
// Vector3d velocityOfTarget;
// box->getLinearVelocity(velocityOfTarget);
// double netVelocityTarget;
// netVelocityTarget=( pow(velocityOfTarget.x(),2) + pow(velocityOfTarget.y(), 2) + pow(velocityOfTarget.z(), 2 ) );
// netVelocityTarget = sqrt(netVelocityTarget);
// stick->getPosition(pos);
// stick->getPartsPosition(pos, "LINK1");
// LOG_MSG((" LINK1 Position is : %f %f %f ", pos.x(), pos.y(), pos.z() ));
// Vector3d targetPos;
// box->getPosition(targetPos);
// Vector3d goalPos;
// goal_001->getPosition(goalPos);
// if (abs( goalPos.z() - targetPos.z()) < 1.4 )
// {
// cout << "The distance to goal is " << abs( goalPos.z() - targetPos.z()) << endl;
// cout << "The goal has been reached " << endl;
// exit(0);
// }
// if (netVelocityTarget > 0.1 )
// {
// double angle = atan ( (targetPos.z() - startPosition.z()) / (targetPos.x() - startPosition.x() ) ) * 180 / PI;
// cout << "The angle is" << angle;
// storePosition(targetPos);
// }
std::vector<std::string> s;
for(std::map<std::string, CParts *>::iterator it = stick->getPartsCollection().begin(); it != stick->getPartsCollection().end(); ++it){
if (it->first != "body")
s.push_back(it->first);
}
std::string linkName;
Size si;
cout << "The total links are " << s.size() << endl;
for (int i = 0; i < s.size(); i++){
const char* linkName = s[i].c_str();
CParts *link = stick->getParts(linkName);
link->getPosition(pos);
link->getRotation(linkRotation);
if (link->getType() == 0){
BoxParts* box = (BoxParts*) link;
si = box->getSize();
// cout << linkName << endl;
cout << linkName << " position : x = " << pos.x() << " y = " << pos.y() << " z = " << pos.z() << endl;
// cout << linkName << " size : x = " << si.x() << " y = " << si.y() << " z = " << si.z() << endl;
// cout << linkName << " Rotation: qw " << linkRotation.qw() << " qx = "<< linkRotation.qx() << " qy = "<< linkRotation.qy()
// << " qz = "<< linkRotation.qz() << endl;
try{
py::object main_module = py::import("__main__");
py::object main_namespace = main_module.attr("__dict__");
main_module.attr("linkName") = linkName;
main_module.attr("length") = si.x();
main_module.attr("height") = si.y();
main_module.attr("breadth") = si.z();
main_module.attr("linkPos") = "[" + tostr(pos.x())+" , "+ tostr(pos.y())+ " , " + tostr(pos.z()) + "]";
main_module.attr("rotation") = "[" + tostr(linkRotation.qw())+" , "+ tostr(linkRotation.qx())+ " , " + tostr(linkRotation.qy()) + " , " + tostr(linkRotation.qz()) +"]";
// calculating the rotation of the tool.
py::exec("import ast", main_namespace);
py::exec("import transformations as T", main_namespace);
py::exec("rotation = ast.literal_eval(rotation)", main_namespace);
// py::exec("angles = T.euler_from_quaternion(rotation) ", main_namespace);
// py::exec("print angles", main_namespace);
py::exec("linkPos = ast.literal_eval(linkPos)", main_namespace);
py::exec_file("vertices.py", main_namespace, main_namespace );
py::exec("getNormals(linkName, linkPos, rotation, length, height, breadth)", main_namespace);
main_module.attr("targetPos") = "[" + tostr(targetPos.x())+" , "+ tostr(targetPos.y())+ " , " + tostr(targetPos.z()) + "]";
main_module.attr("displacementVect") = "[" + tostr(displacementVect.x())+" , "+ tostr(displacementVect.y())+ " , " + tostr(displacementVect.z()) + "]";
py::exec_file("displacementVector.py", main_namespace, main_namespace );
}
catch(boost::python::error_already_set const &){
// Parse and output the exception
std::string perror_str = parse_python_exception();
std::cout << "Error in Python: " << perror_str << std::endl;
}
}
else if(link->getType() == 1){
CylinderParts* cyl = (CylinderParts*) link;
cout << "Cylinder Position : x = " << pos.x() << " y = " << pos.y() << " z = " << pos.z() << endl;
cout << "Cylinder Length : length = " << cyl->getLength() << endl;
cout << "Cylinder Radius : rad = " << cyl->getRad() << endl;
}
else if(link->getType() == 2){
SphereParts* sph = (SphereParts*) link;
cout << "Sphere Position : x = " << pos.x() << " y = " << pos.y() << " z = " << pos.z() << endl;
cout << "Sphere Radius : rad = " << sph->getRad() << endl;
}
}
// stick->getPartsPosition(pos, s[1]);
// stick->getPartsPosition(pos, "LINK1");
// LOG_MSG((" LINK1 Position is : %f %f %f ", pos.x(), pos.y(), pos.z() ));
// stick->getPartsPosition(pos, "LINK2");
// LOG_MSG((" LINK2 Position is : %f %f %f ", pos.x(), pos.y(), pos.z() ));
// stick->getPartsPosition(pos, "LINK3");
// LOG_MSG((" LINK3 Position is : %f %f %f ", pos.x(), pos.y(), pos.z() ));
return 0.00001;
}
double MyController::onAction(ActionEvent &evt)
{
Vector3d pos;
if (start == true){
my->getPosition(pos);
double dx = 0;
double dz = -2.5;
double addx = 0.0;
double addz = 0.0;
if(count%2){
for(int i=0; i<motionNum; i++){
addx += dx;
addz += dz;
if(motionNum)
usleep(sleeptime);
my->setPosition(pos.x()+addx, HEIGHT[i], pos.z()+addz);
my->setJointAngle("LARM_JOINT1", DEG2RAD(LARM_JOINT1[i]));
my->setJointAngle("LARM_JOINT3", DEG2RAD(LARM_JOINT3[i]));
my->setJointAngle("RARM_JOINT1", DEG2RAD(RARM_JOINT1[i]));
my->setJointAngle("RARM_JOINT3", DEG2RAD(RARM_JOINT3[i]));
my->setJointAngle("LLEG_JOINT2", DEG2RAD(LLEG_JOINT2[i]));
my->setJointAngle("LLEG_JOINT4", DEG2RAD(LLEG_JOINT4[i]));
my->setJointAngle("LLEG_JOINT6", DEG2RAD(LLEG_JOINT6[i]));
my->setJointAngle("RLEG_JOINT2", DEG2RAD(RLEG_JOINT2[i]));
my->setJointAngle("RLEG_JOINT4", DEG2RAD(RLEG_JOINT4[i]));
my->setJointAngle("RLEG_JOINT6", DEG2RAD(RLEG_JOINT6[i]));
}
}
else{
for(int i=0; i<motionNum; i++){
addx += dx;
addz += dz;
usleep(sleeptime);
my->setPosition(pos.x()+addx, HEIGHT[i], pos.z()+addz);
my->setJointAngle("RARM_JOINT1", DEG2RAD(LARM_JOINT1[i]));
my->setJointAngle("RARM_JOINT3", DEG2RAD(-LARM_JOINT3[i]));
my->setJointAngle("LARM_JOINT1", DEG2RAD(RARM_JOINT1[i]));
my->setJointAngle("LARM_JOINT3", DEG2RAD(-RARM_JOINT3[i]));
my->setJointAngle("RLEG_JOINT2", DEG2RAD(LLEG_JOINT2[i]));
my->setJointAngle("RLEG_JOINT4", DEG2RAD(LLEG_JOINT4[i]));
my->setJointAngle("RLEG_JOINT6", DEG2RAD(LLEG_JOINT6[i]));
my->setJointAngle("LLEG_JOINT2", DEG2RAD(RLEG_JOINT2[i]));
my->setJointAngle("LLEG_JOINT4", DEG2RAD(RLEG_JOINT4[i]));
my->setJointAngle("LLEG_JOINT6", DEG2RAD(RLEG_JOINT6[i]));
}
}
count++;
if(count==2){
sleep(3);
}
if(count==4){
sendMsg("operator","Passed_through");
}
if(count>step){
start = false;
}
}
return 0.1;
}
double MyController::onAction(ActionEvent &evt)
{
int count = 0;
Vector3d pos;
if (start == true){
int step = 3;
while (count<step){
double dx = 0;
double dz = -2.5;
double addx = 0.0;
double addz = 0.0;
my->getPosition(pos);
for(int i=0; i<motionNum; i++){
addx += dx;
addz += dz;
if(motionNum)
usleep(sleeptime);
my->setPosition(pos.x()+addx, HEIGHT[i], pos.z()+addz);
my->setJointAngle("LARM_JOINT1", DEG2RAD(LARM_JOINT1[i]));
my->setJointAngle("LARM_JOINT3", DEG2RAD(LARM_JOINT3[i]));
my->setJointAngle("RARM_JOINT1", DEG2RAD(RARM_JOINT1[i]));
my->setJointAngle("RARM_JOINT3", DEG2RAD(RARM_JOINT3[i]));
my->setJointAngle("LLEG_JOINT2", DEG2RAD(LLEG_JOINT2[i]));
my->setJointAngle("LLEG_JOINT4", DEG2RAD(LLEG_JOINT4[i]));
my->setJointAngle("LLEG_JOINT6", DEG2RAD(LLEG_JOINT6[i]));
my->setJointAngle("RLEG_JOINT2", DEG2RAD(RLEG_JOINT2[i]));
my->setJointAngle("RLEG_JOINT4", DEG2RAD(RLEG_JOINT4[i]));
my->setJointAngle("RLEG_JOINT6", DEG2RAD(RLEG_JOINT6[i]));
}
for(int i=0; i<motionNum; i++){
addx += dx;
addz += dz;
usleep(sleeptime);
my->setPosition(pos.x()+addx, HEIGHT[i], pos.z()+addz);
my->setJointAngle("RARM_JOINT1", DEG2RAD(LARM_JOINT1[i]));
my->setJointAngle("RARM_JOINT3", DEG2RAD(-LARM_JOINT3[i]));
my->setJointAngle("LARM_JOINT1", DEG2RAD(RARM_JOINT1[i]));
my->setJointAngle("LARM_JOINT3", DEG2RAD(-RARM_JOINT3[i]));
my->setJointAngle("RLEG_JOINT2", DEG2RAD(LLEG_JOINT2[i]));
my->setJointAngle("RLEG_JOINT4", DEG2RAD(LLEG_JOINT4[i]));
my->setJointAngle("RLEG_JOINT6", DEG2RAD(LLEG_JOINT6[i]));
my->setJointAngle("LLEG_JOINT2", DEG2RAD(RLEG_JOINT2[i]));
my->setJointAngle("LLEG_JOINT4", DEG2RAD(RLEG_JOINT4[i]));
my->setJointAngle("LLEG_JOINT6", DEG2RAD(RLEG_JOINT6[i]));
}
count++;
if(count==1){
usleep(3000000);
}
start = false;
}
/*while (count<20){
if (sw == false){
my->getPosition(pos);
my->setPosition(pos.x(), pos.y(), pos.z() - 10);
my->setJointAngle("LARM_JOINT1", DEG2RAD(-30));
my->setJointAngle("RLEG_JOINT2", DEG2RAD(-20));
my->setJointAngle("RLEG_JOINT4", DEG2RAD(10));
usleep(100000);
my->setJointAngle("LARM_JOINT1", DEG2RAD(0));
my->setJointAngle("RLEG_JOINT2", DEG2RAD(0));
my->setJointAngle("RLEG_JOINT4", DEG2RAD(0));
sw = true;
}
else{
my->getPosition(pos);
my->setPosition(pos.x(), pos.y(), pos.z() - 10);
my->setJointAngle("RARM_JOINT1", DEG2RAD(-30));
my->setJointAngle("LLEG_JOINT2", DEG2RAD(-20));
my->setJointAngle("LLEG_JOINT4", DEG2RAD(10));
usleep(100000);
my->setJointAngle("RARM_JOINT1", DEG2RAD(0));
my->setJointAngle("LLEG_JOINT2", DEG2RAD(0));
my->setJointAngle("LLEG_JOINT4", DEG2RAD(0));
sw = false;
}
count++;
if(count==7){
usleep(3000000);
}
start = false;
}*/
}
return 0.1;
}
double MyController::onAction(ActionEvent &evt)
{
// サービスが使用可能か定期的にチェックする
bool available = checkService("CleanUpReferee");
if(!available && m_ref != NULL) m_ref = NULL;
// 使用可能
else if(available && m_ref == NULL){
// サービスに接続
m_ref = connectToService("CleanUpReferee");
}
// 自分の位置取得
Vector3d myPos;
m_my->getPosition(myPos);
// 衝突中の場合,衝突が継続しているかチェック
if(colState){
CParts *parts = m_my->getMainParts();
bool state = parts->getCollisionState();
// 衝突していない状態に戻す
if(!state) colState = false;
}
int entSize = m_entities.size();
for(int i = 0; i < entSize; i++){
// ロボットまたはゴミ箱の場合は除く
if(m_entities[i] == "robot_000" ||
m_entities[i] == "trashbox_0" ||
m_entities[i] == "trashbox_1" ||
m_entities[i] == "trashbox_2"){
continue;
}
// エンティティ取得
SimObj *ent = getObj(m_entities[i].c_str());
// 位置取得
Vector3d tpos;
ent->getPosition(tpos);
// ゴミ箱からゴミを結ぶベクトル
Vector3d vec(tpos.x()-myPos.x(), tpos.y()-myPos.y(), tpos.z()-myPos.z());
// ゴミがゴミ箱の中に入ったかどうか判定
if(abs(vec.x()) < tboxSize_x/2.0 &&
abs(vec.z()) < tboxSize_z/2.0 &&
tpos.y() < tboxMax_y &&
tpos.y() > tboxMin_y ){
// ゴミがリリースされているか確認
if(!ent->getIsGrasped()){
std::string msg;
bool success = false;
// 台の上に置く(成功)
if(strcmp(ent->name(), "mayonaise_0") == 0 && tpos.y() != 57.85) {tpos.y(57.85); success = true;}
else if(strcmp(ent->name(), "chigarette") == 0 && tpos.y() != 54.04){ tpos.y(54.04); success = true;}
else if(strcmp(ent->name(), "chocolate") == 0 && tpos.y() != 51.15){ tpos.y(51.15); success = true;}
else if(strcmp(ent->name(), "mugcup") == 0 && tpos.y() != 54.79){ tpos.y(54.79); success = true;}
else if(strcmp(ent->name(), "petbottle_0") == 0 && tpos.y() != 67.45){ tpos.y(67.45); success = true;}
else if(strcmp(ent->name(), "petbottle_3") == 0 && tpos.y() != 61.95){ tpos.y(61.95); success = true;}
else if(strcmp(ent->name(), "clock") == 0 && tpos.y() != 56.150){ tpos.y(56.150); success = true;}
else if(strcmp(ent->name(), "kettle") == 0 && tpos.y() != 60.650){ tpos.y(60.650); success = true;}
// 台の上に置く(失敗)
else if(strcmp(ent->name(), "petbottle_1") == 0 && tpos.y() != 67.45){ tpos.y(67.45);}
else if(strcmp(ent->name(), "petbottle_2") == 0 && tpos.y() != 67.45){ tpos.y(67.45);}
else if(strcmp(ent->name(), "petbottle_4") == 0 && tpos.y() != 61.95){ tpos.y(61.95);}
else if(strcmp(ent->name(), "mayonaise_1") == 0 && tpos.y() != 57.85){ tpos.y(57.85);}
else if(strcmp(ent->name(), "can_0") == 0 && tpos.y() != 55.335){ tpos.y(55.335);}
else if(strcmp(ent->name(), "can_1") == 0 && tpos.y() != 55.335){ tpos.y(55.335);}
else if(strcmp(ent->name(), "can_2") == 0 && tpos.y() != 57.050){ tpos.y(57.050);}
else if(strcmp(ent->name(), "can_3") == 0 && tpos.y() != 57.050){ tpos.y(57.050);}
else if(strcmp(ent->name(), "banana") == 0 && tpos.y() != 51.69){ tpos.y(51.69);}
else if(strcmp(ent->name(), "apple") == 0 && tpos.y() != 54.675){ tpos.y(54.675);}
else{continue;}
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
ent->setAxisAndAngle(1.0, 0.0, 0.0, 0.0);
ent->setPosition(tpos);
usleep(100000);
if(success){
msg = "CleanUpReferee/";
msg += ent->name();
msg += " succeeded/1000";
}
else{
msg = "CleanUpReferee/";
msg += ent->name();
msg += " failed/-600";
}
if(m_ref != NULL) {
m_ref->sendMsgToSrv(msg.c_str());
}
else{
LOG_MSG((msg.c_str()));
}
}
}
}
return retValue;
}
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;
}