Пример #1
0
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]);
		}
	}
}
Пример #3
0
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)); 
}  
Пример #5
0
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;
}
Пример #6
0
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;
}
Пример #9
0
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;
}
Пример #10
0
void UserController::moveBody(int task)
{
	SimObj *my = getObj(myname());

	my->setJointAngle("WAIST_JOINT1", tab_joint_body[task]);
}
Пример #11
0
void UserController::moveHead(int task)
{
	SimObj *my = getObj(myname());

	my->setJointAngle("HEAD_JOINT0", tab_joint_head[task]);
}