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