double MyController::rotateTowardGrabPos(Vector3d pos, double velocity, double now)
{
printf("start rotate %lf \n", now);
//自分を取得
SimObj *my = getObj(myname());
//printf("向く座標 %lf %lf %lf \n", pos.x(), pos.y(), pos.z());
// 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
//自分の手のパーツを得ます
CParts * parts = my->getParts("RARM_LINK7");
Vector3d partPos;
parts->getPosition(partPos);
// 自分の位置からターゲットを結ぶベクトル
Vector3d tmpp = pos;
//tmpp -= myPos;
tmpp -= partPos;
// y方向は考えない
tmpp.y(0);
// 自分の回転を得る
Rotation myRot;
m_my->getRotation(myRot);
// エンティティの初期方向
Vector3d iniVec(0.0, 0.0, 1.0);
// y軸の回転角度を得る(x,z方向の回転は無いと仮定)
double qw = myRot.qw();
double qy = myRot.qy();
double theta = 2*acos(fabs(qw));
//printf("qw: %lf theta: %lf \n", qw, theta);
if(qw*qy < 0) {
//printf("qw * qy < 0 \n");
theta = -1*theta;
}
// z方向からの角度
//printf("結ぶベクトル 座標 %lf %lf %lf \n", tmpp.x(), tmpp.y(), tmpp.z());
double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
//printf("tmp: %lf \n", tmp);
double targetAngle = acos(tmp);
//printf("targetAngle: %lf ---> %lf \n", targetAngle, targetAngle*180.0/PI);
// 方向
//printf("tmpp.x() %lf \n", tmpp.x());
if(tmpp.x() > 0) {
targetAngle = -1*targetAngle;
//printf("targetAngle: %lf deg \n", targetAngle*180.0/PI);
}
targetAngle += theta;
//printf("targetAngle: %lf <--- %lf \n", targetAngle, theta);
//printf("qw: %lf qy: %lf theta: %lf tmp: %lf targetAngle: %lf \n", qw, qy, theta, tmp, targetAngle);
if(targetAngle == 0.0){
//printf("donot need rotate \n");
return 0.0;
}
else
{
// 回転すべき円周距離
double distance = m_distance*PI*fabs(targetAngle)/(2*PI);
// 車輪の半径から移動速度を得る
double vel = m_radius*velocity;
// 回転時間(u秒)
double time = distance / vel;
// 車輪回転開始
if(targetAngle > 0.0){
m_my->setWheelVelocity(velocity, -velocity);
}
else
{
m_my->setWheelVelocity(-velocity, velocity);
}
//printf("distance: %lf vel: %lf time: %lf \n", distance, vel, time);
printf("rotate time: %lf, time to stop: %lf \n", time, now + time);
return now + time;
}
}
double MyController::onAction(ActionEvent &evt)
{
if(!checkService("RecogTrash")){
m_srv == NULL;
}
if(m_srv == NULL){
// ゴミ認識サービスが利用可能か調べる
if(checkService("RecogTrash")){
// ゴミ認識サービスに接続
m_srv = connectToService("RecogTrash");
}
}
//if(evt.time() < m_time) printf("state: %d \n", m_state);
switch(m_state) {
// 初期状態
case 0: {
if(m_srv == NULL){
// ゴミ認識サービスが利用可能か調べる
if(checkService("RecogTrash")){
// ゴミ認識サービスに接続
m_srv = connectToService("RecogTrash");
}
} else if(m_srv != NULL && m_executed == false){
//rotate toward upper
m_my->setJointVelocity("LARM_JOINT4", -m_jvel, 0.0);
m_my->setJointVelocity("RARM_JOINT4", -m_jvel, 0.0);
// 50°回転
m_time = DEG2RAD(ROTATE_ANG) / m_jvel + evt.time();
m_state = 5;
m_executed = false;
}
break;
}
case 5: {
if(evt.time() > m_time && m_executed == false) {
//m_my->setJointVelocity("LARM_JOINT1", 0.0, 0.0);
m_my->setJointVelocity("LARM_JOINT4", 0.0, 0.0);
m_my->setJointVelocity("RARM_JOINT4", 0.0, 0.0);
sendSceneInfo("Start");
//m_srv->sendMsgToSrv("Start");
printf("Started! \n");
m_executed = true;
}
break;
}
// 物体の方向が帰ってきた
case 20: {
// 送られた座標に回転する
m_time = rotateTowardObj(nextPos, m_rotateVel, evt.time());
//printf("debug %lf %lf \n", evt.time(), m_time);
m_state = 21;
m_executed = false;
break;
}
case 21: {
// ロボットが回転中
//printf("debug %lf %lf \n", evt.time(), m_time);
if(evt.time() > m_time && m_executed == false) {
// 物体のある場所に到着したので、車輪と止め、関節を回転し始め、物体を拾う
m_my->setWheelVelocity(0.0, 0.0);
// 物体のある方向に回転した
//printf("目的地の近くに移動します %lf %lf %lf \n", nextPos.x(), nextPos.y(), nextPos.z());
// 送られた座標に移動する
m_time = goToObj(nextPos, m_vel*4, m_range, evt.time());
//m_time = goToObj(nextPos, m_vel*4, 40, evt.time());
m_state = 22;
m_executed = false;
}
break;
}
case 22: {
// 送られた座標に移動した
if(evt.time() > m_time && m_executed == false) {
m_my->setWheelVelocity(0.0, 0.0);
printf("止める \n");
Vector3d myPos;
m_my->getPosition(myPos);
double x = myPos.x();
double z = myPos.z();
double theta = 0; // y方向の回転は無しと考える
char replyMsg[256];
bool found = recognizeNearestTrash(m_tpos, m_tname);
// ロボットのステートを更新
if (found == true) {
m_state = 500;
m_executed = false;
//printf("m_executed = false, state = 500 \n");
} else {
//printf("Didnot found anything \n");
m_state = 10;
m_executed = false;
}
}
break;
}
case 30: {
// 送られた座標に回転する
m_time = rotateTowardObj(nextPos, m_rotateVel, evt.time());
//printf("case 30 time: %lf \n", m_time);
m_state = 31;
m_executed = false;
break;
}
// 物体を掴むために、ロボットの向く角度をズラス
case 31: {
if(evt.time() > m_time && m_executed == false) {
Vector3d grabPos;
//printf("斜めにちょっとずれた以前の時間 time: %lf \n", evt.time());
if(calcGrabPos(nextPos, 20, grabPos)) {
m_time = rotateTowardObj(grabPos, m_vel / 5, evt.time());
printf("斜め grabPos :%lf %lf %lf \n", grabPos.x(), grabPos.y(), grabPos.z());
printf("time: %lf \n", m_time);
}
m_state = 32;
m_executed = false;
}
break;
}
// 物体の方向が帰ってきた
case 32: {
if(evt.time() > m_time && m_executed == false) {
// 物体のある場所に到着したので、車輪と止め、関節を回転し始め、物体を拾う
m_my->setWheelVelocity(0.0, 0.0);
//printf("回転を止めた evt.time %lf \n", evt.time());
// 関節の回転を始める
m_my->setJointVelocity("RARM_JOINT1", -m_jvel, 0.0);
// 50°回転
m_time = DEG2RAD(ROTATE_ANG) / m_jvel + evt.time();
m_state = 33;
m_executed = false;
}
break;
}
case 33: {
// 関節回転中
if(evt.time() > m_time && m_executed == false) {
// 関節の回転を止める
m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0);
// 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
double x = myPos.x();
double z = myPos.z();
double theta = 0; // y方向の回転は無しと考える
//物体を掴めるか掴めないかによって処理を分岐させる
if(m_grasp) { // 物体を掴んだ
// 捨てたゴミをゴミ候補
std::vector<std::string>::iterator it;
// ゴミ候補を得る
it = std::find(m_trashes.begin(), m_trashes.end(), m_tname);
// 候補から削除する
m_trashes.erase(it);
printf("erased ... \n");
// ゴミ箱への行き方と問い合わせする
char replyMsg[256];
// もっとも近いゴミ箱を探す
//bool found = recognizeNearestTrashBox(m_trashBoxPos, m_trashBoxName);
// ゴミを置くべき座標を探す
bool found = findPlace2PutObj(m_trashBoxPos, m_tname);
if(found) {
// ゴミ箱が検出出来た
std::cout << "trashboxName " << m_trashBoxName << std::endl;
sprintf(replyMsg, "AskTrashBoxRoute %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf",
x, z, theta, m_trashBoxPos.x(), m_trashBoxPos.y(), m_trashBoxPos.z());
} else {
sprintf(replyMsg, "AskTrashBoxPos %6.1lf %6.1lf %6.1lf",
x, z, theta);
}
m_srv->sendMsgToSrv(replyMsg);
m_executed = true;
} else { // 物体を掴めなかった、次に探す場所を問い合わせる
// ゴミを掴めなかったもしくはゴミが無かった、次にゴミのある場所を問い合わせする
// 逆方向に関節の回転を始める
m_my->setJointVelocity("RARM_JOINT1", m_jvel, 0.0);
// 50°回転
m_time = DEG2RAD(ROTATE_ANG) / m_jvel + evt.time();
m_state = 34;
m_lastFailedTrash = m_tname;
m_executed = false;
}
}
break;
}
case 34: {
if(evt.time() > m_time && m_executed == false) {
// 関節の回転を止める
m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0);
// 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
double x = myPos.x();
double z = myPos.z();
double theta = 0;
char replyMsg[256];
sprintf(replyMsg, "AskObjPos %6.1lf %6.1lf %6.1lf", x, z, theta);
printf("case 34 debug %s \n", replyMsg);
m_srv->sendMsgToSrv(replyMsg);
m_executed = true;
}
break;
}
case 40: {
// 送られた座標に回転する
m_time = rotateTowardObj(nextPos, m_rotateVel, evt.time());
m_state = 41;
m_executed = false;
break;
}
case 41: {
// 送られた座標に回転中
if(evt.time() > m_time && m_executed == false) {
// 送られた座標に移動する
printf("目的地の近くに移動します %lf %lf %lf \n", nextPos.x(), nextPos.y(), nextPos.z());
m_time = goToObj(nextPos, m_vel*4, m_range, evt.time());
m_state = 42;
m_executed = false;
}
break;
}
case 42: {
// 送られた座標に移動中
if(evt.time() > m_time && m_executed == false) {
// 送られた座標に到着した、 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
double x = myPos.x();
double z = myPos.z();
double theta = 0; // y方向の回転は無しと考える
char replyMsg[256];
// もっとも近いゴミ箱を探す
bool found = recognizeNearestTrashBox(m_trashBoxPos, m_trashBoxName);
if(found) {
// ゴミ箱が検出出来た
std::cout << "trashboxName " << m_trashBoxName << std::endl;
sprintf(replyMsg, "AskTrashBoxRoute %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf",
x, z, theta, m_trashBoxPos.x(), m_trashBoxPos.y(), m_trashBoxPos.z());
} else {
sprintf(replyMsg, "AskTrashBoxPos %6.1lf %6.1lf %6.1lf",
x, z, theta);
}
m_srv->sendMsgToSrv(replyMsg);
m_executed = true;
}
break;
}
case 50: {
if(evt.time() > m_time && m_executed == false) {
Vector3d throwPos;
//printf("斜めにちょっとずれた以前の時間 time: %lf \n", evt.time());
// 送られた座標に到着した、 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
printf("robot pos %lf %lf \n", myPos.x(), myPos.z());
// grasp中のパーツを取得します
CParts *parts = m_my->getParts("RARM_LINK7");
// grasp中のパーツの座標を取得出来れば、回転する角度を逆算出来る。
Vector3d partPos;
if (parts->getPosition(partPos)) {
printf("parts pos before rotate %lf %lf %lf \n", partPos.x(), partPos.y(), partPos.z());
}
//if(calcGrabPos(nextPos, 20, throwPos)) {
if(calcGrabPos(nextPos, 20, throwPos)) {
m_time = rotateTowardObj(throwPos, m_vel / 5, evt.time());
printf("斜めに捨てる throwPos :%lf %lf %lf \n", throwPos.x(), throwPos.y(), throwPos.z());
//printf("time: %lf \n", m_time);
}
m_state = 51;
m_executed = false;
}
break;
}
case 51: {
// ゴミ箱に到着したので、車輪を停止し、アームを下ろし、物体をゴミ箱に捨てる準備をする
m_my->setWheelVelocity(0.0, 0.0);
// grasp中のパーツを取得します
CParts *parts = m_my->getParts("RARM_LINK7");
// grasp中のパーツの座標を取得出来れば、回転する角度を逆算出来る。
Vector3d partPos;
if (parts->getPosition(partPos)) {
printf("parts pos after rotate %lf %lf %lf \n", partPos.x(), partPos.y(), partPos.z());
}
// releaseします
parts->releaseObj();
// ゴミが捨てられるまで少し待つ
sleep(1);
// grasp終了
m_grasp = false;
//confirmThrewTrashPos(m_threwPos, m_tname);
//printf("捨てた座標： %lf %lf %lf \n", m_threwPos.x(), m_threwPos.y(), m_threwPos.z());
// 関節の回転を始める
m_my->setJointVelocity("RARM_JOINT1", m_jvel, 0.0);
m_time = DEG2RAD(ROTATE_ANG) / m_jvel + evt.time() + 1.0;
m_state = 52;
m_executed = false;
break;
}
case 52: {
// 関節が回転中
if(evt.time() > m_time && m_executed == false) {
// 関節が元に戻った、関節の回転を止める
m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0);
// 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
double x = myPos.x();
double z = myPos.z();
double theta = 0; // y方向の回転は無しと考える
// ゴミを捨てたので、次にゴミのある場所を問い合わせする
char replyMsg[256];
if(recognizeNearestTrash(m_tpos, m_tname)) {
m_executed = false;
// 物体が発見された
m_state = 500;
} else {
sprintf(replyMsg, "AskObjPos %6.1lf %6.1lf %6.1lf", x, z, theta);
m_srv->sendMsgToSrv(replyMsg);
m_executed = true;
}
}
break;
}
case 100: {
m_my->setJointVelocity("RARM_JOINT1", 0.0, 0.0);
m_my->setWheelVelocity(0.0, 0.0);
break;
}
case 800: {
if(evt.time() > m_time && m_executed == false) {
sendSceneInfo();
m_executed = true;
}
break;
}
case 805: {
if(evt.time() > m_time && m_executed == false) {
// 送られた座標に移動する
double range = 0;
m_time = rotateTowardObj(nextPos, m_rotateVel, evt.time());
m_state = 807;
m_executed = false;
}
break;
}
case 807: {
if(evt.time() > m_time && m_executed == false) {
m_my->setWheelVelocity(0.0, 0.0);
printf("移動先 x: %lf, z: %lf \n", nextPos.x(), nextPos.z());
m_time = goToObj(nextPos, m_vel*4, m_range, evt.time());
m_state = 810;
m_executed = false;
}
break;
}
case 810: {
// 送られた座標に移動中
if(evt.time() > m_time && m_executed == false) {
m_my->setWheelVelocity(0.0, 0.0);
m_time = rotateTowardObj(m_lookingPos, m_rotateVel, evt.time());
m_executed = false;
m_state = 815;
}
break;
}
case 815: {
// 送られた座標に移動中
if(evt.time() > m_time && m_executed == false) {
m_my->setWheelVelocity(0.0, 0.0);
sendSceneInfo();
printf("sent data to SIGViewer \n");
m_executed = true;
}
break;
}
case 920: {
// 送られた座標に回転する
m_time = rotateTowardObj(nextPos, m_rotateVel, evt.time());
m_state = 921;
m_executed = false;
break;
}
case 921: {
// ロボットが回転中
if(evt.time() > m_time && m_executed == false) {
m_my->setWheelVelocity(0.0, 0.0);
m_executed = false;
}
break;
}
default: {
break;
}
}
return 0.05;
}
double MyController::onAction(ActionEvent &evt) {
dlopen("libpython2.7.so", RTLD_LAZY | RTLD_GLOBAL);
Py_Initialize(); //initialization of the python interpreter
//return 1.0;
SimObj *stick = getObj("robot_test");
SimObj *box = getObj("box_001");
SimObj *goal_001 = getObj("checkpoint_001");
double torque;
// The target position
Vector3d targetPos;
box->getPosition(targetPos);
// The Goal Position: The sphere is placed at Goal position.
Vector3d goalPos;
goal_001->getPosition(goalPos);
// calculating the displacement vector
Vector3d displacementVect;
displacementVect.x(goalPos.x()-targetPos.x());
displacementVect.y(goalPos.y()-targetPos.y());
displacementVect.z(goalPos.z()-targetPos.z());
// stick->addForce(-500,0,500); // This adds force to on the stick tool.
// stick->addForce(0,0,5000);
// Vector3d angularVel;
// stick->getAngularVelocity(angularVel);
// LOG_MSG((" Current angular Velocity is : %f %f %f ", angularVel.x(), angularVel.y(), angularVel.z() ));
// if ( abs(angularVel.y()) > 0.1 )
// {
// LOG_MSG((" Current angular Velocity is : %f %f %f ", angularVel.x(), angularVel.y(), angularVel.z() ));
// double* ptr1 = NULL;
// ptr1 = controlAngularVelocity(angularVel, 3.0, 0, 1.0);
// torque = ptr1[0] * 500;
// // torque = ptr1[0] * 12000 ;
// cout << "The torque applied for controlling angular velocity is " << torque << " N. m" << endl;
// stick->addTorque( 0 , torque, 0);
// }
// to control the rotation of the tool
// Rotation currentToolRot;
// stick->getRotation(currentToolRot);
// double *ptr = NULL;
// ptr = controlRotation(initialToolRot, currentToolRot, 20.0, 0.0, 20.0);
// torque = ptr[1] * 200 ;
// cout << "The torque applied for controlling the rotation = " << torque << endl;
// stick->addTorque(0, torque,0);
// double massOfTool;
// massOfTool = stick->getMass();
// cout << "The mass is " << massOfTool <<endl;
// Vector3d velocityOfTarget;
// box->getLinearVelocity(velocityOfTarget);
// double netVelocityTarget;
// netVelocityTarget=( pow(velocityOfTarget.x(),2) + pow(velocityOfTarget.y(), 2) + pow(velocityOfTarget.z(), 2 ) );
// netVelocityTarget = sqrt(netVelocityTarget);
// stick->getPosition(pos);
// stick->getPartsPosition(pos, "LINK1");
// LOG_MSG((" LINK1 Position is : %f %f %f ", pos.x(), pos.y(), pos.z() ));
// Vector3d targetPos;
// box->getPosition(targetPos);
// Vector3d goalPos;
// goal_001->getPosition(goalPos);
// if (abs( goalPos.z() - targetPos.z()) < 1.4 )
// {
// cout << "The distance to goal is " << abs( goalPos.z() - targetPos.z()) << endl;
// cout << "The goal has been reached " << endl;
// exit(0);
// }
// if (netVelocityTarget > 0.1 )
// {
// double angle = atan ( (targetPos.z() - startPosition.z()) / (targetPos.x() - startPosition.x() ) ) * 180 / PI;
// cout << "The angle is" << angle;
// storePosition(targetPos);
// }
std::vector<std::string> s;
for(std::map<std::string, CParts *>::iterator it = stick->getPartsCollection().begin(); it != stick->getPartsCollection().end(); ++it){
if (it->first != "body")
s.push_back(it->first);
}
std::string linkName;
Size si;
cout << "The total links are " << s.size() << endl;
for (int i = 0; i < s.size(); i++){
const char* linkName = s[i].c_str();
CParts *link = stick->getParts(linkName);
link->getPosition(pos);
link->getRotation(linkRotation);
if (link->getType() == 0){
BoxParts* box = (BoxParts*) link;
si = box->getSize();
// cout << linkName << endl;
cout << linkName << " position : x = " << pos.x() << " y = " << pos.y() << " z = " << pos.z() << endl;
// cout << linkName << " size : x = " << si.x() << " y = " << si.y() << " z = " << si.z() << endl;
// cout << linkName << " Rotation: qw " << linkRotation.qw() << " qx = "<< linkRotation.qx() << " qy = "<< linkRotation.qy()
// << " qz = "<< linkRotation.qz() << endl;
try{
py::object main_module = py::import("__main__");
py::object main_namespace = main_module.attr("__dict__");
main_module.attr("linkName") = linkName;
main_module.attr("length") = si.x();
main_module.attr("height") = si.y();
main_module.attr("breadth") = si.z();
main_module.attr("linkPos") = "[" + tostr(pos.x())+" , "+ tostr(pos.y())+ " , " + tostr(pos.z()) + "]";
main_module.attr("rotation") = "[" + tostr(linkRotation.qw())+" , "+ tostr(linkRotation.qx())+ " , " + tostr(linkRotation.qy()) + " , " + tostr(linkRotation.qz()) +"]";
// calculating the rotation of the tool.
py::exec("import ast", main_namespace);
py::exec("import transformations as T", main_namespace);
py::exec("rotation = ast.literal_eval(rotation)", main_namespace);
// py::exec("angles = T.euler_from_quaternion(rotation) ", main_namespace);
// py::exec("print angles", main_namespace);
py::exec("linkPos = ast.literal_eval(linkPos)", main_namespace);
py::exec_file("vertices.py", main_namespace, main_namespace );
py::exec("getNormals(linkName, linkPos, rotation, length, height, breadth)", main_namespace);
main_module.attr("targetPos") = "[" + tostr(targetPos.x())+" , "+ tostr(targetPos.y())+ " , " + tostr(targetPos.z()) + "]";
main_module.attr("displacementVect") = "[" + tostr(displacementVect.x())+" , "+ tostr(displacementVect.y())+ " , " + tostr(displacementVect.z()) + "]";
py::exec_file("displacementVector.py", main_namespace, main_namespace );
}
catch(boost::python::error_already_set const &){
// Parse and output the exception
std::string perror_str = parse_python_exception();
std::cout << "Error in Python: " << perror_str << std::endl;
}
}
else if(link->getType() == 1){
CylinderParts* cyl = (CylinderParts*) link;
cout << "Cylinder Position : x = " << pos.x() << " y = " << pos.y() << " z = " << pos.z() << endl;
cout << "Cylinder Length : length = " << cyl->getLength() << endl;
cout << "Cylinder Radius : rad = " << cyl->getRad() << endl;
}
else if(link->getType() == 2){
SphereParts* sph = (SphereParts*) link;
cout << "Sphere Position : x = " << pos.x() << " y = " << pos.y() << " z = " << pos.z() << endl;
cout << "Sphere Radius : rad = " << sph->getRad() << endl;
}
}
// stick->getPartsPosition(pos, s[1]);
// stick->getPartsPosition(pos, "LINK1");
// LOG_MSG((" LINK1 Position is : %f %f %f ", pos.x(), pos.y(), pos.z() ));
// stick->getPartsPosition(pos, "LINK2");
// LOG_MSG((" LINK2 Position is : %f %f %f ", pos.x(), pos.y(), pos.z() ));
// stick->getPartsPosition(pos, "LINK3");
// LOG_MSG((" LINK3 Position is : %f %f %f ", pos.x(), pos.y(), pos.z() ));
return 0.00001;
}
