bool RobotController::moveArm(int left_or_right)
{
std::string jointName;
bool finishFlag[8] = {false,false,false,false,false,false,false,false};
if(left_or_right == LEFT_ARM)
{
jointName = "LARM_JOINT";
}
else if(left_or_right == RIGHT_ARM)
{
jointName = "RARM_JOINT";
}
for ( int i = 0; i < 8; i++ )
{
if (armJoint[left_or_right][i] != targetArmJoint[left_or_right][i] )
{
std::stringstream ss;
ss << jointName << i;
if (armJoint[left_or_right][i] < targetArmJoint[left_or_right][i] && targetArmJoint[left_or_right][i]-armJoint[left_or_right][i] > ERROR_ANGLE_ARM)
{
my->setJointVelocity(ss.str().c_str(), joint_veloc, 0.0);
armJoint[left_or_right][i] = my->getJointAngle(ss.str().c_str());
}
else if (armJoint[left_or_right][i] > targetArmJoint[left_or_right][i] && armJoint[left_or_right][i]-targetArmJoint[left_or_right][i] > ERROR_ANGLE_ARM)
{
my->setJointVelocity(ss.str().c_str(), -joint_veloc, 0.0);
armJoint[left_or_right][i] = my->getJointAngle(ss.str().c_str());
}
else
{
my->setJointVelocity(ss.str().c_str(), 0.0, 0.0);
finishFlag[i] = true;
}
}
else finishFlag[i] = true;
}
for( int i = 0; i < 8; i++)
{
if(!finishFlag[i]) return false;
}
return true;
}
void DemoRobotController::neutralizeArms(double evt_time)
{
double angleJoint1 = m_robotObject->getJointAngle("RARM_JOINT1")*180.0/(M_PI);
double angleJoint4 = m_robotObject->getJointAngle("RARM_JOINT4")*180.0/(M_PI);
double thetaJoint1 = -15 - angleJoint1;
double thetaJoint4 = -110 - angleJoint4;
if(thetaJoint4<0) m_robotObject->setJointVelocity("RARM_JOINT4", -m_jointVelocity, 0.0);
else m_robotObject->setJointVelocity("RARM_JOINT4", m_jointVelocity, 0.0);
if(thetaJoint1<0) m_robotObject->setJointVelocity("RARM_JOINT1", -m_jointVelocity, 0.0);
else m_robotObject->setJointVelocity("RARM_JOINT1", m_jointVelocity, 0.0);
m_time1 = DEG2RAD(abs(thetaJoint1))/ m_jointVelocity + evt_time;
m_time4 = DEG2RAD(abs(thetaJoint4))/ m_jointVelocity + evt_time;
}
double RobotController::onAction(ActionEvent &evt)
{
switch(m_state){
// 初期姿勢を設定 seting initial pose
case 0: {
//broadcastMsgToSrv("Let's start the clean up task\n");
sendMsg("VoiceReco_Service","Let's start the clean up task\n");
double angL1 =m_my->getJointAngle("LARM_JOINT1")*180.0/(PI);
double angL4 =m_my->getJointAngle("LARM_JOINT4")*180.0/(PI);
double angR1 =m_my->getJointAngle("RARM_JOINT1")*180.0/(PI);
double angR4 =m_my->getJointAngle("RARM_JOINT4")*180.0/(PI);
double thetaL1 = -20-angL1;
double thetaL4 = -160-angL4;
double thetaR1 = -20-angR1;
double thetaR4 = -160-angR4;
if(thetaL1<0) m_my->setJointVelocity("LARM_JOINT1", -m_jvel, 0.0);
else m_my->setJointVelocity("LARM_JOINT1", m_jvel, 0.0);
if(thetaL4<0) m_my->setJointVelocity("LARM_JOINT4", -m_jvel, 0.0);
else m_my->setJointVelocity("LARM_JOINT4", m_jvel, 0.0);
if(thetaR1<0) m_my->setJointVelocity("RARM_JOINT1", -m_jvel, 0.0);
else m_my->setJointVelocity("RARM_JOINT1", m_jvel, 0.0);
if(thetaR4<0) m_my->setJointVelocity("RARM_JOINT4", -m_jvel, 0.0);
else m_my->setJointVelocity("RARM_JOINT4", m_jvel, 0.0);
m_time_LA1 = DEG2RAD(abs(thetaL1))/ m_jvel + evt.time();
m_time_LA4 = DEG2RAD(abs(thetaL4))/ m_jvel + evt.time();
m_time_RA1 = DEG2RAD(abs(thetaR1))/ m_jvel + evt.time();
m_time_RA4 = DEG2RAD(abs(thetaR4))/ m_jvel + evt.time();
m_state = 1;
break;
}
// 初期姿勢に移動 moving initial pose
case 1: {
if(evt.time() >= m_time_LA1) m_my->setJointVelocity("LARM_JOINT1", 0.0, 0.0);
if(evt.time() >= m_time_LA4) m_my->setJointVelocity("LARM_JOINT4", 0.0, 0.0);
if(evt.time() >= m_time_RA1) m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0);
if(evt.time() >= m_time_RA4) m_my->setJointVelocity("RARM_JOINT4", 0.0, 0.0);
if(evt.time() >= m_time_LA1 && evt.time() >= m_time_LA4
&& evt.time() >= m_time_RA1 && evt.time() >= m_time_RA4){
// 位置Aの方向に回転を開始します setting position a for rotating
//broadcastMsgToSrv("Moving to the table");
m_time = rotateTowardObj(pos_a, m_vel, evt.time());
m_state = 2;
}
break;
}
// 位置Aの方向に回転 rotating to position a
case 2: {
// 回転終了
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
// 位置Aに移動します setting position a for moving
m_time = goToObj(pos_a, m_vel*4, 0.0, evt.time());
m_state = 3;
}
break;
}
// 位置Aに移動 moving to position a
case 3: {
// 位置Aに到着
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
// 位置Bの方向に回転を開始します setting position b for rotating
m_time = rotateTowardObj(pos_b, m_vel, evt.time());
m_state = 4;
}
break;
}
// 位置Bの方向に回転 rotating to position b
case 4: {
// 回転終了
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
// 位置Bに移動します setting position b for moving
m_time = goToObj(pos_b, m_vel*4, 0.0, evt.time());
m_state = 5;
}
break;
}
// 位置Bに移動 moving to position b
case 5: {
// 位置Bに到着
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
// テーブルの方向に回転を開始します setting table position for rotating
SimObj *table = getObj("table_0");
Vector3d pos;
table->getPosition(pos);
m_time = rotateTowardObj(pos, m_vel, evt.time());
m_state = 6;
}
break;
}
// テーブルの方向に回転 rotating to table
case 6: {
// 回転終了
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
// ゴミがある場所と名前を取得します
// ゴミが見つからなかった
if(!this->recognizeTrash(m_tpos,m_tname)){
//broadcastMsgToSrv("No trash detected");
//broadcastMsgToSrv("Task finished");
sleep(10);
}
// ゴミが見つかった trash detected
else{
//broadcastMsgToSrv("Please show which trash to take\n");
sendMsg("VoiceReco_Service","Please show me which object to take");
m_state = 7;
}
}
break;
}
// wating to point
case 7: {
break;
}
// 物体認識開始 starting object recognition
case 8: {
// m_tpos object direction on object
SimObj *target = this->getObj(m_pointedObject.c_str());
target->getPosition(m_tpos);
//broadcastMsgToSrv("Ok I will take it\n");
msg_ob = "I will take " + m_pointedObject ;
sendMsg("VoiceReco_Service",msg_ob);
// ゴミの方向に回転をはじめる
m_time = rotateTowardObj(m_tpos, m_vel, evt.time());
m_state = 9;
break;
}
// ゴミの方向に回転をはじめる setting trash position for rotating
case 9: {
m_time = rotateTowardObj(m_tpos, m_vel, evt.time());
m_state = 10;
break;
}
// ゴミの方向に回転中 rotating to trash
case 10: {
// 回転終了
if(evt.time() >= m_time){
// 回転を止める
m_my->setWheelVelocity(0.0, 0.0);
//ゴミの位置まで移動をはじめる setting trash position for moving
m_time = goToObj(m_tpos, m_vel*4, 25.0, evt.time());
m_state = 11;
}
break;
}
// ゴミの位置まで移動中 moving to trash
case 11: {
// 移動終了
if(evt.time() >= m_time){
// 移動を止める
m_my->setWheelVelocity(0.0, 0.0);
// 関節の回転を始める setting arm for grasping
double angR1 =m_my->getJointAngle("RARM_JOINT1")*180.0/(PI);
double angR4 =m_my->getJointAngle("RARM_JOINT4")*180.0/(PI);
double thetaR1 = -30.0-angR1;
double thetaR4 = 0.0-angR4;
if(thetaR1<0) m_my->setJointVelocity("RARM_JOINT1", -m_jvel, 0.0);
else m_my->setJointVelocity("RARM_JOINT1", m_jvel, 0.0);
if(thetaR4<0) m_my->setJointVelocity("RARM_JOINT4", -m_jvel, 0.0);
else m_my->setJointVelocity("RARM_JOINT4", m_jvel, 0.0);
m_time_RA1 = DEG2RAD(abs(thetaR1) )/ m_jvel + evt.time();
m_time_RA4 = DEG2RAD(abs(thetaR4) )/ m_jvel + evt.time();
// ゴミを取りに関節を曲げる状態に移行します
m_state = 12;
}
break;
}
// 関節を回転中 rotating arm for grasping
case 12: {
// 関節回転終了
if(evt.time() >= m_time_RA1) m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0);
if(evt.time() >= m_time_RA4) m_my->setJointVelocity("RARM_JOINT4", 0.0, 0.0);
if(evt.time() >= m_time_RA1 && evt.time() >= m_time_RA4){
if(m_grasp) {
//broadcastMsgToSrv("grasping the trash");
// 関節の回転を始める setting arm for taking
double angR4 =m_my->getJointAngle("RARM_JOINT4")*180.0/(PI);
double thetaR4 = -90.0-angR4;
if(thetaR4<0) m_my->setJointVelocity("RARM_JOINT4", -m_jvel, 0.0);
else m_my->setJointVelocity("RARM_JOINT4", m_jvel, 0.0);
m_time_RA4 = DEG2RAD(abs(thetaR4) )/ m_jvel + evt.time();
// 関節を戻す状態に移行します
m_state = 13;
}
else{
// graspできない
broadcastMsgToSrv("Unreachable");
}
}
break;
}
// 関節を回転中 rotating arm for taking
case 13: {
// 関節回転終了
if(evt.time() >= m_time_RA4){
m_my->setJointVelocity("RARM_JOINT4", 0.0, 0.0);
// 位置Aの方向に回転を開始します setting position a for rotating
//broadcastMsgToSrv("Moving to the trashbox");
sendMsg("VoiceReco_Service","Now I will go to the trash boxes");
m_time = rotateTowardObj(pos_a, m_vel, evt.time());
m_state = 14;
}
break;
}
// 位置Aの方向に回転 rotating to position a
case 14: {
// 回転終了
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
// 位置Aに移動します setting position a for moving
m_time = goToObj(pos_a, m_vel*4, 0.0, evt.time());
m_state = 15;
}
break;
}
// 位置Aの位置まで移動中 movig to position a
case 15: {
// 移動終了
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
//broadcastMsgToSrv("Please tell me which trash box \n");
sendMsg("VoiceReco_Service","Please show me which trash box to use");
m_state = 16;
}
break;
}
// watig to point4
case 16: {
break;
}
// ゴミ箱認識 starting trash box recognitiong-g-0
case 17: {
// m_tpos object direction on object
SimObj *target_trash = this->getObj(m_pointedtrash.c_str());
target_trash->getPosition(m_tpos);
//broadcastMsgToSrv("Ok I will throw the trash in trash box \n");
msg_trash = "Ok I will put "+ m_pointedObject+"in"+ m_pointedtrash + "\n";
sendMsg("VoiceReco_Service",msg_trash);
// ゴミの方向に回転をはじめる setting position trash box for rotating
m_time = rotateTowardObj(m_tpos, m_vel, evt.time());
m_state = 18;
break;
}
// ゴミ箱の方向に回転中 rotating to trash box
case 18: {
if(evt.time() >= m_time){
// 回転を止める
m_my->setWheelVelocity(0.0, 0.0);
//ゴミの位置まで移動をはじめる setting trash position for moving
m_time = goToObj(m_tpos, m_vel*4, 30.0, evt.time());
m_state = 19;
}
break;
}
// ゴミを持ってゴミ箱に向かっている状態 moving to trash box
case 19: {
// ゴミ箱に到着
if(evt.time() >= m_time){
m_my->setWheelVelocity(0.0, 0.0);
// grasp中のパーツを取得します getting grasped tarts
CParts *parts = m_my->getParts("RARM_LINK7");
// releaseします
parts->releaseObj();
// ゴミが捨てられるまで少し待つ
sleep(1);
// 捨てたゴミをゴミ候補から削除 deleting grasped object from list
std::vector<std::string>::iterator it;
it = std::find(m_trashes.begin(), m_trashes.end(), m_pointedObject);
m_trashes.erase(it);
// grasp終了
m_grasp = false;
m_state = 1;
}
break;
}
}
return 0.01;
}
