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]);
}
