void MyController::onRecvMsg(RecvMsgEvent &evt) { //取得したメッセージを表示します string msg = evt.getMsg(); //LOG_MSG(("msg : %s", msg.c_str())); SimObj *my = getObj(myname()); if(strstr(msg.c_str()," ")) { // phi theta に分ける int n = 0; n = msg.find(" "); string phi_s = msg.substr(0,n); string theta_s = msg.substr(n+1); double phi = atof(theta_s.c_str()); double theta = atof(phi_s.c_str()); // 目玉関節を回転させる my->setJointAngle("LEYE_JOINT1",DEG2RAD(phi)); my->setJointAngle("REYE_JOINT1",DEG2RAD(phi)); my->setJointAngle("LEYE_JOINT0",DEG2RAD(theta)); my->setJointAngle("REYE_JOINT0",DEG2RAD(theta)); } }
void MyController::onInit(InitEvent &evt) { my = getObj(myname()); start = false; sw = false; // 手を下げる my->setJointAngle("LARM_JOINT2", DEG2RAD(-90)); my->setJointAngle("RARM_JOINT2", DEG2RAD(90)); 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]); } } }
void UserController::moveRightArm(int task) { SimObj *my = getObj(myname()); my->setJointAngle("RARM_JOINT0", tab_joint_right[task][0]); my->setJointAngle("RARM_JOINT1", tab_joint_right[task][1]); my->setJointAngle("RARM_JOINT2", tab_joint_right[task][2]); my->setJointAngle("RARM_JOINT3", tab_joint_right[task][3]); my->setJointAngle("RARM_JOINT4", tab_joint_right[task][4]); my->setJointAngle("RARM_JOINT5", tab_joint_right[task][5]); my->setJointAngle("RARM_JOINT6", tab_joint_right[task][6]); my->setJointAngle("RARM_JOINT7", tab_joint_right[task][7]); }
void MyController::onInit(InitEvent &evt) { my = getObj(myname()); start =false; sw=false; // 左手を下に下げます my->setJointAngle("LARM_JOINT2", DEG2RAD(-90)); // 右手をsageます my->setJointAngle("RARM_JOINT2", DEG2RAD(90)); }
double NiiRobotOkonomi::onAction(ActionEvent &evt) { SimObj *my = getObj(myname()); static int s_cnt = 0; int step = s_cnt % STEP_NUM; for (int i=0; i<JOINT_NUM; i++) { const char * jointName = jointNames[i]; if (!jointName) { continue; } double angle = jointAngles[step][i]; double rad = DEG2RAD(angle); my->setJointAngle(jointName, rad); } s_cnt++; /* double zz = cos(s_cnt*5*3.14/180); double xx = sin(s_cnt*5*3.14/180); my->setPosition(xx, 0.5, zz); my->setAxisAndAngle(0.0, 1.0, 0.0, s_cnt*5*3.14/180); */ return 0.1; }
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) { 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; }
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) { 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; }
void UserController::moveBody(int task) { SimObj *my = getObj(myname()); my->setJointAngle("WAIST_JOINT1", tab_joint_body[task]); }
void UserController::moveHead(int task) { SimObj *my = getObj(myname()); my->setJointAngle("HEAD_JOINT0", tab_joint_head[task]); }