void MyController::onCollision(CollisionEvent &evt) {
Colli = true;
counterOfCollision ++ ;
// handle of target and tool
// handle of target and tool
SimObj *toolName = getObj("TShapeTool_7");
SimObj *target = getObj("box_7");
if(Colli && counterOfCollision == 1 )
{
toolName->getVelocity(toolVelAtHit);
target->getVelocity(targetVelAtHit);
cout << "The tool velocity is " << toolVelAtHit.x() << " , " << toolVelAtHit.z() << endl;
cout << "The target velocity is " << targetVelAtHit.x() << " , " << targetVelAtHit.z() << endl;
// myfile << toolVelAtHit.x() << " , " << toolVelAtHit.z() << " , " ;
// myfile << targetVelAtHit.x() << " , " << targetVelAtHit.z() << " , " ;
Colli = false;
}
}
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;
}
bool RecvEntitiesEvent::set(int packetNum, int seq, char *data, int n)
{
char *p = data;
m_time = BINARY_GET_DOUBLE_INCR(p);
for (;;) {
int head = p - data;
int left = n - head;
if (left <= 0) {
fprintf(stderr, "RecvEntitiesEvent : packet too small left = %d\n", left);
return false;
}
if (CommData::isPacketEnd(p)) {
break;
}
SimObj *obj = new SimObj();
int r = obj->setBinary(p, left);
if (r < 0) { return false; }
push(obj);
p += r;
}
return true;
}
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));
}
}
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;
}
void MyController::onCollision(CollisionEvent &evt)
{
if (m_grasp == false){
typedef CollisionEvent::WithC C;
//触れたエンティティの名前を得ます
const std::vector<std::string> & with = evt.getWith();
// 衝突した自分のパーツを得ます
const std::vector<std::string> & mparts = evt.getMyParts();
// 衝突したエンティティでループします
for(int i = 0; i < with.size(); i++){
//右手に衝突した場合
if(mparts[i] == "RARM_LINK7"){
//自分を取得
SimObj *my = getObj(myname());
//自分の手のパーツを得ます
CParts * parts = my->getParts("RARM_LINK7");
parts->graspObj(with[i]);
m_grasp = true;
}
}
}
}
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 UserController::onInit(InitEvent &evt)
{
robotName = "robot_000";
//m_kinect = connectToService("SIGKINECT");
//m_hmd = connectToService("SIGHMD");
parseFile("command.txt");
Mission_complete = false;
//printf("Reslutat %s", rooms[2]);
rooms.clear();
objects.clear();
m_message = 10 ;
cycle = 0;
m_state =20;
srand(time(NULL));
//初期位置の設定
SimObj *my = this->getObj(this->myname());
m_posx = my->x();
m_posy = my->y();
m_posz = my->z();
m_range = 0.1;
m_maxsize = 15;
}
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]);
}
}
}
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;
}
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);
}
double RobotController::onAction(ActionEvent &evt)
{
SimObj *obj = getObj(myname());
if (obj) {
obj->setAccel(0.0, 100.0, 0.0);
}
return 5.0;
}
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 MyController::onInit(InitEvent &evt) {
SimObj *stick = getObj("robot_test");
// stick->addForce(-5000,0,5000);
stick->getPosition(startPosition);
// stick->getRotation(initialToolRot);
}
void WheelControllerByMyo::onRecvMsg(RecvMsgEvent &evt)
{
const std::string allMsg = evt.getMsg();
// std::cout << allMsg << std::endl;
std::map<std::string, std::vector<std::string> > sensorDataMap = MyoSensorData::decodeSensorData(allMsg);
if (sensorDataMap.find(MSG_KEY_DEV_TYPE) == sensorDataMap.end()){ return; }
if(sensorDataMap[MSG_KEY_DEV_TYPE][0] !=this->myoDeviceManager.deviceType ){ return; }
if(sensorDataMap[MSG_KEY_DEV_UNIQUE_ID][0]!=this->myoDeviceManager.deviceUniqueID){ return; }
MyoSensorData sensorData;
sensorData.setSensorData(sensorDataMap);
// std::cout << "roll=" << sensorData.roll << ",pitch=" << sensorData.pitch << ",yaw=" << sensorData.yaw << ",pose=" << sensorData.poseStr << std::endl;
// std::cout << "emg1=" << sensorData.emgData[0] << ",emg2=" << sensorData.emgData[1] << ", emg3=" << sensorData.emgData[2] << std::endl;
SimObj *obj = getObj(myname());
float vel = 0.0;
for(int i=0; i<MyoSensorData::EMG_SENSOR_NUM; i++)
{
vel += std::abs(sensorData.emgData[i]);
}
double rvel=0.0, lvel=0.0;
// Turn left.
if(sensorData.poseStr==MyoSensorData::mapPoseType2Str.at(MyoSensorData::WaveIn))
{
rvel = 3.0*vel;
lvel = 0.05*vel;
}
// Turn right.
if(sensorData.poseStr==MyoSensorData::mapPoseType2Str.at(MyoSensorData::WaveOut))
{
rvel = 0.05*vel;
lvel = 3.0*vel;
}
// Brake.
if(sensorData.poseStr==MyoSensorData::mapPoseType2Str.at(MyoSensorData::Fist))
{
rvel = -3.0*vel;
lvel = -3.0*vel;
}
// std::cout << "rvel=" << rvel << ",lvel=" << lvel << std::endl;
obj->setJointVelocity("JOINT_RWHEEL", rvel, 500.0);
obj->setJointVelocity("JOINT_LWHEEL", lvel, 500.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));
}
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;
}
void Simulation::cycle()
{
//if ((mSimulationTime % 1000) == 0) Gridbrain::debugMutationsCount();
bool draw = mDrawGraphics && mDrawThisCycle;
list<SimObj*>::iterator iterObj;
for (iterObj = mObjectsToKill.begin();
iterObj != mObjectsToKill.end();
iterObj++)
{
mPopulationDynamics->onOrganismDeath(*iterObj);
}
mObjectsToKill.clear();
if (draw)
{
drawBeforeObjects();
}
onCycle();
for (iterObj = mObjects.begin(); iterObj != mObjects.end(); ++iterObj)
{
SimObj* obj = *iterObj;
obj->process();
}
for (iterObj = mObjects.begin(); iterObj != mObjects.end(); ++iterObj)
{
SimObj* obj = *iterObj;
if (obj->mType == SimObj::TYPE_AGENT)
{
obj->perceive();
obj->emptyMessageList();
obj->compute();
obj->act();
}
if (draw)
{
obj->draw();
}
}
if (draw)
{
drawAfterObjects();
}
mPopulationDynamics->onCycle(mSimulationTime, art_getTime());
mSimulationTime++;
}
double DetectEntitiesController::onAction(ActionEvent &evt)
{
std::vector<std::string> agents;
std::vector<std::string>::iterator i;
static int count = 0;
// LOG_MSG(("bear onAction(count=%d)\n", count));
bool b = detectEntities(agents);
if (b)
{
int n = agents.size();
if (n>0)
{
LOG_MSG(("%d entities detected", n));
for (int i=0; i<n; i++)
{
std::string name = agents[i];
LOG_MSG(("[%d] (%s)", i, name.c_str()));
}
}
}
else
{
LOG_MSG(("detectEntities failed"));
}
try {
SimObj *o = getObj(myname());
if (o && !o->dynamics())
{
double deg = count * 10.0;
double rad = -3.141592/180*deg;
o->setAxisAndAngle(0.0, 1.0, 0.0, rad);
}
} catch(SimObj::NoAttributeException &) {
} catch(SimObj::AttributeReadOnlyException &) {
} catch(SimObj::Exception &) {
}
count++;
return 2.0;
}
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;
}
double MyController::onAction(ActionEvent &evt) {
SimObj *obj = getObj(myname()); //obtaining handle to the agent
obj->setLinearVelocity(0,0,100); //apply the linear velocity 20[m/s] in Z axis
Vector3d currentVelocity;
obj->getLinearVelocity(currentVelocity);
if (myfile.is_open() && (evt.time() < 15) )
{
myfile << currentVelocity.x() << " , " << currentVelocity.z() << "\n" ;
}
if(evt.time() > 15)
{
exit(0);
}
return 0.00001;
}
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 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();
}
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");
}
}
}
/* ゴミの名前から、捨てるべきゴミ箱の名前を検索し、そのゴミ箱の位置を探す
* @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::onRecvMsg(RecvMsgEvent &evt)
{
string msg = evt.getMsg();
if(msg == "walk"){
start = true;
}
if(msg == "Task_start"){
start = false;
my->setPosition(initPos);
count = 0;
}
}
double RobotController::onAction(ActionEvent &evt)
{
LOG_MSG(("\ncurrent time : %f", evt.time()));
static int cnt = 0;
try {
const char *name = myname();
SimObj *obj = getObj(name);
obj->dump();
if (!obj->dynamics()) {
double angle = 2*PI*cnt*0.01;
double xx = 5*sin(angle);
double yy = 0.5;
double zz = 5*cos(angle);
LOG_MSG(("pos (%f, %f, %f)", xx, yy, zz));
obj->setPosition(xx, yy, zz);
obj->setAxisAndAngle(0.0, 1.0, 0.0, angle);
}
obj->dump();
} catch(SimObj::NoAttributeException &) {
} catch(SimObj::AttributeReadOnlyException &) {
} catch(SimObj::Exception &) {
}
cnt++;
return 0.1;
}
//定期的に呼び出される関数です。
double CameraController::onAction(ActionEvent &evt)
{
// ロボット位置取得
r_my->getPosition(r_pos);
// カメラ番号取得
m_my = getObj(myname());
m_name = m_my->name();
// 定点カメラをロボットの方向に回転
rotateTowardRobot(r_pos);
return 0.05;
}
void DemoRobotController::onCollision(CollisionEvent &evt)
{
if (m_grasp == false) {
typedef CollisionEvent::WithC C;
// Get name of entity which is touched by the robot
const std::vector<std::string> & with = evt.getWith();
// Get parts of the robot which is touched by the entity
const std::vector<std::string> & mparts = evt.getMyParts();
// loop for every collided entities
for(int i = 0; i < with.size(); i++) {
if(m_graspObjectName == with[i]) {
// If the right hand touches the entity
if(mparts[i] == "RARM_LINK7") {
SimObj *my = getObj(myname());
CParts * parts = my->getParts("RARM_LINK7");
if(parts->graspObj(with[i])) m_grasp = true;
}
}
}
}
}