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