void MyController::onRecvMsg(RecvMsgEvent &evt)
{
std::string msg = evt.getMsg();
if(msg == "Task_start"){
if(trials==0){
m_state = 100;
}
else{
m_state = 1100;
}
broadcastMsg("get message: Task_start");
}
else if(msg == "Task_end"){
m_my->setWheelVelocity(0.0, 0.0);
m_state = 0;
broadcastMsg("get message: Task_end");
trials++;
}
else if(msg == "Time_over"){
m_my->setWheelVelocity(0.0, 0.0);
m_state = 0;
}
/*else if(m_state == 207 && msg == "leave the elevator"){
broadcastMsg("get message: leave the elevator");
m_state = 208;
}*/
}
void MyController::onInit(InitEvent &evt)
{
m_my = getRobotObj(myname());
// 初期位置取得
m_my->getPosition(m_inipos);
// 車輪の半径と車輪間隔距離
m_radius = 10.0;
m_distance = 10.0;
m_time = 0.0;
// 車輪の半径と車輪間距離設定
m_my->setWheel(m_radius, m_distance);
m_state = 0;
// ここではゴミの名前が分かっているとします
m_trashes.push_back("can_0");
m_trashes.push_back("can_1");
m_trashes.push_back("petbottle_0");
srand((unsigned)time( NULL ));
// 車輪の回転速度
m_vel = 0.3;
// 関節の回転速度
m_jvel = 0.6;
}
// move to a target point
double MyController::goToObj(Vector3d pos, double velocity, double range, double now)
{
// get own position
Vector3d myPos;
m_my->getPosition(myPos);
// vector from own position to a target position
pos -= myPos;
pos.y(0);
// distance to a target position
double distance = pos.length() - range;
// calcurate veloocity from radius of wheels
double vel = m_radius*velocity;
// start moving
m_my->setWheelVelocity(velocity, velocity);
// calcurate time of arrival
double time = distance / vel;
return now + time;
}
double DemoRobotController::goToObj(Vector3d pos, double range)
{
// get own position
Vector3d robotCurrentPosition;
//m_robotObject->getPosition(robotCurrentPosition);
m_robotObject->getPartsPosition(robotCurrentPosition,"RARM_LINK2");
// pointing vector for target
Vector3d l_pos = pos;
l_pos -= robotCurrentPosition;
// ignore y-direction
l_pos.y(0);
// measure actual distance
double distance = l_pos.length() - range;
// start moving
m_robotObject->setWheelVelocity(m_angularVelocity, m_angularVelocity);
// time to be elapsed
double l_time = distance / m_movingSpeed;
return l_time;
}
// object まで移動
double MyController::goToObj(Vector3d nextPos, double velocity, double range, double now)
{
printf("goToObj関数内 goToObj %lf %lf %lf \n", nextPos.x(), nextPos.y(), nextPos.z());
// 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
printf("goToObj関数内 ロボットの現在位置: x: %lf, z %lf \n", myPos.x(), myPos.z());
// 自分の位置からターゲットを結ぶベクトル
Vector3d pos = nextPos;
pos -= myPos;
// y方向は考えない
pos.y(0);
// 距離計算
double distance = pos.length() - range;
//printf("distance: %lf \n", distance);
//printf("range = %lf \n", range);
// 車輪の半径から移動速度を得る
double vel = m_radius*velocity;
// 移動開始
m_my->setWheelVelocity(velocity, velocity);
// 到着時間取得
double time = distance / vel;
return now + time;
}
// object まで移動
double MyController::goToObj(Vector3d pos, double velocity, double range, double now)
{
// 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
// 自分の位置からターゲットを結ぶベクトル
pos -= myPos;
// y方向は考えない
pos.y(0);
// 距離計算
double distance = pos.length() - range;
// 車輪の半径から移動速度を得る
double vel = m_radius*velocity;
// 移動開始
m_my->setWheelVelocity(velocity, velocity);
// 到着時間取得
double time = distance / vel;
return now + time;
}
void MyController::setRobotPosition(double x, double z) {
Vector3d myPos;
m_my->getPosition(myPos);
double y = myPos.y();
m_my->setPosition(x, y, z);
return;
}
double DemoRobotController::rotateTowardObj(Vector3d pos)
{
// "pos" means target position
// get own position
Vector3d ownPosition;
m_robotObject->getPartsPosition(ownPosition,"RARM_LINK2");
// pointing vector for target
Vector3d l_pos = pos;
l_pos -= ownPosition;
// ignore variation on y-axis
l_pos.y(0);
// get own rotation matrix
Rotation ownRotation;
m_robotObject->getRotation(ownRotation);
// get angles arround y-axis
double qw = ownRotation.qw();
double qy = ownRotation.qy();
double theta = 2*acos(fabs(qw));
if(qw*qy < 0) theta = -1.0*theta;
// rotation angle from z-axis to x-axis
double tmp = l_pos.angle(Vector3d(0.0, 0.0, 1.0));
double targetAngle = acos(tmp);
// direction
if(l_pos.x() > 0) targetAngle = -1.0*targetAngle;
targetAngle += theta;
double angVelFac = 3.0;
double l_angvel = m_angularVelocity/angVelFac;
if(targetAngle == 0.0) {
return 0.0;
}
else {
// circumferential distance for the rotation
double l_distance = m_distance*M_PI*fabs(targetAngle)/(2.0*M_PI);
// Duration of rotation motion (micro second)
double l_time = l_distance / (m_movingSpeed/angVelFac);
// Start the rotation
if(targetAngle > 0.0) {
m_robotObject->setWheelVelocity(l_angvel, -l_angvel);
}
else{
m_robotObject->setWheelVelocity(-l_angvel, l_angvel);
}
return l_time;
}
}
void DemoRobotController::prepareThrowing(double evt_time)
{
double thetaJoint1 = 50.0;
m_robotObject->setJointVelocity("RARM_JOINT1", -m_jointVelocity, 0.0);
m_time1 = DEG2RAD(abs(thetaJoint1))/ m_jointVelocity + evt_time;
double thetaJoint4 = 65.0;
m_robotObject->setJointVelocity("RARM_JOINT4", m_jointVelocity, 0.0);
m_time4 = DEG2RAD(abs(thetaJoint4))/ m_jointVelocity + evt_time;
}
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;
}
void MyController::setCameraPosition(double angle, int camID) {
double xDir = sin(DEG2RAD(angle));
double zDir = cos(DEG2RAD(angle));
m_my->setCamDir(Vector3d(xDir, 0.0, zDir), camID);
printf("camera Dir converted \n");
return;
}
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 RobotController::release_left_hand()
{
CParts * parts = my->getParts("LARM_LINK7");
if ( m_grasp_left == true){
printf("I'm releasing \n");
}
parts->releaseObj();
m_grasp_left = false;
}
void RobotController::onInit(InitEvent &evt)
{
joint_veloc = 0.8;
my = getRobotObj(myname());
// 車輪間距離
m_distance = 10.0;
// 車輪半径
m_radius = 10.0;
// 車輪の半径と車輪間距離設定
my->setWheel(m_radius, m_distance);
m_grasp_left = false;
Change_Robot_speed = 5; // to change the robot's velocity
Robot_speed = Change_Robot_speed;
m_state = 0;
// objects position
m_BottleFront = Vector3d(40.0, 30, -40.0);
m_MuccupFront = Vector3d(0.0, 30, -40.0);
m_CanFront = Vector3d(-40.0, 30, -40.0);
// trashBoxs position
m_BurnableFront = Vector3d(-120.0, 30, -60);
m_UnburnableFront = Vector3d(-120.0, 30, 70);
m_RecycleFront = Vector3d(-60.0, 30, -100);
m_relayPoint1 = Vector3d(100, 30, -70);
m_relayPoint2 = Vector3d(0, 30, -70);
m_relayFrontTable = Vector3d(0, 30,-20);
m_relayFrontTable_reset = Vector3d(-80, 30,-50);
m_relayFrontTrash = Vector3d(-80, 30, -80);
cycle = 27;
m_onActionReturn = 0.01;
srand((unsigned)time( NULL ));
// grasp初期化
m_grasp = false;
m_trashes.push_back("petbottle");
m_trashes.push_back("mugcup");
m_trashes.push_back("can");
// ゴミ箱登録
m_trashboxs.push_back("recycle");
m_trashboxs.push_back("burnable");
m_trashboxs.push_back("unburnable");
}
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;
}
double DemoRobotController::goGraspingObject(Vector3d &pos)
{
double l_time;
double thetaJoint4 = 20.0;
m_robotObject->setJointVelocity("RARM_JOINT4", m_jointVelocity, 0.0);
l_time = DEG2RAD(abs(thetaJoint4))/ m_jointVelocity;
return l_time;
}
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");
}
}
}
//定期的に呼び出される関数です。
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 MyController::onInit(InitEvent &evt)
{
m_my = getRobotObj(myname());
// 初期位置取得
m_my->getPosition(m_inipos);
//v3の追加点
// ゴミがある方向にカメラを向ける
m_my->setCamDir(Vector3d(0.0, -1.0, 1.0),1);
m_my->setCamDir(Vector3d(0.0, -1.0, 1.0),2);
// 車輪の半径と車輪間隔距離
m_radius = 10.0;
m_distance = 10.0;
m_time = 0.0;
// 車輪の半径と車輪間距離設定
m_my->setWheel(m_radius, m_distance);
m_state = 0;
srand((unsigned)time( NULL ));
// 車輪の回転速度
m_vel = 1.0; //1.0;
m_rotateVel = 0.6; //1.0;
// 関節の回転速度
m_jvel = 0.6;
m_lookObjFlg = 0.0;
// grasp初期化
m_grasp = false;
m_srv = NULL;
m_sended = false;
m_executed = false;
}
void MyController::onInit(InitEvent &evt)
{
start = false;
elevator = false;
crowd = false;
end = false;
stop = false;
first = false;
i=0;
m_my = getRobotObj(myname());
// 初期位置取得
m_my->getPosition(m_inipos);
// 車輪の半径と車輪間隔距離
m_radius = 10.0;
m_distance = 10.0;
m_time = 0.0;
// 車輪の半径と車輪間距離設定
m_my->setWheel(m_radius, m_distance);
m_state = 0;
srand((unsigned)time( NULL ));
// 車輪の回転速度
m_vel = 0.3;
// 関節の回転速度
m_jvel = 0.6;
}
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_name = m_my->name();
// 定点カメラをロボットの方向に回転
rotateTowardRobot(r_pos);
}
void DemoRobotController::throwTrash(void)
{
// get the part info.
CParts *parts = m_robotObject->getParts("RARM_LINK7");
// release grasping
parts->releaseObj();
// wait a bit
sleep(1);
// set the grasping flag to neutral
m_grasp = false;
}
double MyController::calcHeadingAngle()
{
// 自分の回転を得る
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;
}
return theta * 180.0 / PI;
}
//定期的に呼び出される関数です。
double CameraController::onAction(ActionEvent &evt)
{
// ロボット位置取得
r_my->getPosition(r_pos);
// カメラ番号取得
m_my = getObj(myname());
m_name = m_my->name();
// std::cout << "configuringPath : " << configuringPath << std::endl;
if (!configuringPath) {
// 定点カメラをロボットの方向に回転
// rotateTowardRobot(r_pos);
}
return 0.05;
}
char* MyController::sendSceneInfo(std::string header, int camID) {
m_my->setWheelVelocity(0.0, 0.0);
Vector3d myPos;
m_my->getPosition(myPos);
double x = myPos.x();
double z = myPos.z();
double theta = calcHeadingAngle(); // y方向の回転は無しと考える
// カメラがついているリンク名取得
std::string link = ""; //"WAIST_LINK0"; //m_my->getCameraLinkName(3);
if (camID < 3) {
link = m_my->getCameraLinkName(camID);
} else {
link = "WAIST_LINK0";
}
//const dReal *lpos = m_my->getParts(link.c_str())->getPosition();
Vector3d lpos;
m_my->getParts(link.c_str())->getPosition(lpos);
// カメラの位置取得(リンク座標系)
Vector3d cpos;
m_my->getCamPos(cpos, camID);
// カメラの位置(絶対座標系, ロボットの回転はないものとする)
printf("linkpos: %lf %lf %lf \n", lpos.x(), lpos.y(), lpos.z());
printf("camerapos: %lf %lf %lf \n", cpos.x(), cpos.y(), cpos.z());
Vector3d campos(lpos.x() + cpos.z() * sin(DEG2RAD(theta)),
lpos.y() + cpos.y(),
lpos.z() + cpos.z() * cos(DEG2RAD(theta)));
//Vector3d campos(cpos.x(), cpos.y(), cpos.z());
// カメラの方向取得(ロボットの回転,関節の回転はないものとする)
Vector3d cdir;
m_my->getCamDir(cdir, camID);
char *replyMsg = new char[1024];
sprintf(replyMsg, "%s %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf",
header.c_str(), x, z, theta, campos.x(), campos.y(), campos.z(), cdir.x(), cdir.y(), cdir.z());
printf("%s \n", replyMsg);
m_srv->sendMsgToSrv(replyMsg);
m_my->setWheelVelocity(0.0, 0.0);
return replyMsg;
}
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 DemoRobotController::onInit(InitEvent &evt)
{
// get robot's name
m_robotObject = getRobotObj(myname());
// set wheel configuration
m_radius = 10.0;
m_distance = 10.0;
m_robotObject->setWheel(m_radius, m_distance);
m_time = 0.0;
m_time1 = 0.0;
m_time4 = 0.0;
m_state = 10; // switch of initial behavior
refreshRateOnAction = 0.1; // refresh-rate for onAction proc.
// angular velocity of wheel and moving speed of robot
m_angularVelocity = 1.5;
m_movingSpeed = m_angularVelocity*m_radius; // conversion: rad/ms -> m/ms)
// rotation speed of joint
m_jointVelocity = 0.5;
m_trashName1 = "petbottle_1";
m_trashName2 = "can_0";
m_trashBoxName1 = "trashbox_0"; // for recycle
m_trashBoxName2 = "trashbox_1"; // for burnable
// set positions;
m_frontTrashBox1 = Vector3d(-80.0, 0.0, -90); // for recycle material
m_frontTrashBox2 = Vector3d( 20.0, 0.0, -90); // for burnable material
m_relayPoint1 = Vector3d(190.0, 0.0, -65.0);
m_frontTrash1 = Vector3d(273.0, 0.0, -65.0);
m_frontTrash2 = Vector3d(305.0, 0.0, -80.0);
m_grasp = false;
}
bool MyController::calcGrabPos(Vector3d pos, double robotShoulderWidth, Vector3d &grabPos)
{
// ロボットの幅の半分 16.5cm
robotShoulderWidth = 16.5;
//printf("物体の座標 %lf %lf %lf \n", pos.x(), pos.y(), pos.z());
// 物体のある高さを保存暗記する
double grabX = 0, grabY = pos.y(), grabZ = 0;
// 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
//printf("ロボットの位置 %lf %lf %lf \n", myPos.x(), myPos.y(), myPos.z());
// 自分の位置からターゲットを結ぶベクトル
pos -= myPos;
//printf("結ぶ座標 %lf %lf %lf \n", pos.x(), pos.y(), pos.z());
double dis = sqrt(pos.x() * pos.x() + pos.z() * pos.z());
if(dis > robotShoulderWidth) {
// 物体を掴むために、ロボットが結ぶベクトルからズレる角度
double rate = robotShoulderWidth / dis;
double ang = asin(rate);
//printf("angle: %lf \n", ang * 180 / PI);
grabX = cos(ang) * pos.x() + sin(ang) * pos.z();
grabZ = cos(ang) * pos.z() - sin(ang) * pos.x();
//printf("grabX %lf, grabZ %lf, myPos.x %lf, myPos.z %lf \n", grabX, grabZ, myPos.x(), myPos.z());
// ロボットが向くべき座標
grabX += myPos.x();
grabZ += myPos.z();
grabPos.set(grabX, grabY, grabZ);
printf("向くべき座標 %lf %lf %lf \n", grabPos.x(), grabPos.y(), grabPos.z());
return true;
} else {
return false;
}
return false;
}
void MyController::onInit(InitEvent &evt)
{
m_my = getRobotObj(myname());
// radius and distance of wheel
m_radius = 10.0;
m_distance = 10.0;
// time to stop moving
m_time = 0.0;
// set radius and distance of wheel
m_my->setWheel(m_radius, m_distance);
// angular velocity of wheel
m_vel = 0.3 * 10;
m_vel_rot = 0.3;
// state of robot
m_state = 0;
trials = 0; // for demonstration
}
double MyController::rotateTowardObj(Vector3d pos, double velocity, double now)
{
// 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
// 自分の位置からターゲットを結ぶベクトル
Vector3d tmpp = pos;
tmpp -= myPos;
// 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));
if(qw*qy < 0)
theta = -1*theta;
// z方向からの角度
double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
double targetAngle = acos(tmp);
// 方向
if(tmpp.x() > 0) targetAngle = -1*targetAngle;
targetAngle += theta;
if(targetAngle == 0.0){
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);
}
return now + time;
}
}
