color_generator(double s, double v) : s(s), v(v) {
std::uniform_real_distribution<double> unif(0., 1.);
std::random_device rand_dev;
std::mt19937 rand_engine(rand_dev());
h = unif(rand_engine);
}
// sum = 1
void randinit_w(std::vector<std::vector<double> > &points, int dimension, double lower_bound, double upper_bound)
{
std::uniform_real_distribution<double> unif(lower_bound, upper_bound);
std::random_device rand_dev; // Use random_device to get a random seed.
std::mt19937 rand_engine(rand_dev()); // mt19937 is a good pseudo-random number
// generator.
for(int i = 0; i < points.size();i++){
std::vector<double> v(dimension);
double sum = 0.0;
for(int j = 0; j < dimension; j++){
v[j] = (unif(rand_engine));
sum += v[j];
}
double unit = 1.0 / sum;
sum = 0;
for(int j = 0; j < dimension; j++){
v[j] *= unit;
sum += v[j];
}
points[i] = v;
}
}
/*get a rand double value*/
double get_rand(double lower_bound, double upper_bound)
{
std::uniform_real_distribution<double> unif(lower_bound, upper_bound);
std::random_device rand_dev; // Use random_device to get a random seed.
std::mt19937 rand_engine(rand_dev()); // mt19937 is a good pseudo-random number
// generator.
return unif(rand_engine);
}
void Engine::InitializeEnemyShip()
{
const int range_from = 0;
const int range_to = WINDOW_SIZE.x;
std::random_device rand_dev;
std::mt19937 generator(rand_dev());
std::uniform_int_distribution<int> distr(range_from, range_to);
EnemyShip* enemy = new EnemyShip();
enemy->SetPosition(sf::Vector2f(distr(generator), -30.f));
enemyShips.push_back(enemy);
}
/*Uniform distribution data*/
void randinit(std::vector<std::vector<double> > &points, int dimension, double lower_bound, double upper_bound)
{
std::uniform_real_distribution<double> unif(lower_bound, upper_bound);
std::random_device rand_dev; // Use random_device to get a random seed.
std::mt19937 rand_engine(rand_dev()); // mt19937 is a good pseudo-random number
// generator.
for(int i = 0; i < points.size();i++){
std::vector<double> v;
for(int j = 0; j < dimension; j++)
v.push_back(unif(rand_engine));
points[i] = v;
}
}
std::vector<const Point<T>*> uniformPoints(int count,
int dim, T min, T max){
std::vector<const Point<T>*> points(count);
std::random_device rand_dev;
std::mt19937 gen(rand_dev());
std::uniform_real_distribution<> dist(min, max);
// Generate count points
for(int i = 0;i < count; i++){
Point<T>* p = new Point<T>(dim);
// Generate each dimension.
for(int j = 0; j < dim; j++){
(*p)[j] = dist(gen);
}
points[i] = p;
}
return points;
}
int main(){
//class
KinectMangerThread kinectManager;
RobotArm arm;
InvalidMotionHandler motionHandler;
ColorBasedTracker tracker;
//variable
cv::Rect RobotROI((KINECT_DEPTH_WIDTH - 160) / 2 + 40, (KINECT_DEPTH_HEIGHT- 160) / 2, 160, 160);
bool saveCheck = false;
int sampleAngleBox[9], count = 0;
const int sampleAngleLimit[9] = {10000, 10000, 10000, 10000, 10000, 10000, 400, 400, 400};
char dirName[256];
float averBox[NUM_XEL] = {0};
//uniform random sampler
const int range_from = 0;
const int range_to = 10;
std::random_device rand_dev;
std::mt19937 generator(rand_dev());
std::uniform_int_distribution<int> distr_arm(-5000, 5000);
std::uniform_int_distribution<int> distr_finger(-200, 200);
//initialize
kinectManager.Initialize(RobotROI);
motionHandler.Initialize();
int presentSecond = time(NULL);
#ifdef RIGHT_ARM_USE
dxl_write_dword(arm.DXL_Get_Port(), 7, NX::P_HOMING_OFFSET_LL, 62750, 0);
#elif defined LEFT_ARM_USE
dxl_write_dword(arm.DXL_Get_Port(), 8, NX::P_HOMING_OFFSET_LL, -62750, 0);
#endif
ControllerInit(&arm);
if(!motionHandler.robotConnect(&arm)){
printf("Robot can not connect.\n");
motionHandler.Deinitialize();
return -1;
}
arm.TorqueOff();
printf("If u want to start program, press any key.\n");
getch();
//디렉토리 생성
itoa(presentSecond, dirName, 10);
CreateRGBDdir(dirName);
//배경취득
cv::Mat backRGB = kinectManager.getImg();
cv::Mat backDepth = kinectManager.getDepth();
cv::imshow("background", backRGB);
cv::waitKey(1);
char buf[256];
strcpy(buf, DEFAULT_PATH);
strcat(buf, dirName);
sprintf(buf, "%s\\%s", DEFAULT_PATH, dirName);
writeDepthData(backDepth, buf, "backDepth");
strcat(buf, "\\backRGB.bmp");
cv::imwrite(buf, backRGB);
tracker.InsertBackGround(backRGB, backDepth);
printf("\nIf u save background, press any key\n");
getch();
arm.TorqueOn();
while(!kinectManager.isThreadDead()){
//샘플링된 모션이 가능한 모션인지를 체크
int getAngle[9], tmpAngle[9], completePosition[9];
//motionHandler.ForwardEnd(&arm);
printf("Sampling start....");
while(1){
arm.GetPresPosition(getAngle);
//angle sampling - limit 이내의 각도 샘플링
memcpy(tmpAngle, getAngle, sizeof(int) * 9);
for(int i = 0; i < NUM_XEL; i++){
if(i < NUM_JOINT) sampleAngleBox[i] = distr_arm(generator);
else sampleAngleBox[i] = distr_finger(generator);
tmpAngle[i] += sampleAngleBox[i];
}
//motion check
if(motionHandler.InvalidCheck(tmpAngle, getAngle))
break;
}
printf("complete!\n");
printf("sampling result : \n");
for(int i = 0; i < NUM_XEL; i++) printf("%d ", sampleAngleBox[i]);
printf("\n");
//실제 움직임
printf("Move Robot.....");
arm.SetGoalPosition(tmpAngle);
WaitUntilMoveEnd(&arm);
arm.GetPresPosition(completePosition);
printf("complete!\n");
//write file
cv::Mat img = kinectManager.getImg();
cv::Mat depth = kinectManager.getDepth();
cv::Mat pointCloud = kinectManager.getPointCloud();
cv::imshow("cropImg", img);
cv::waitKey(1);
if(writeData(img, depth, pointCloud, &tracker, completePosition, dirName, count)){
count++;
printf("[%d] data saveComplete.\n", count);
printf("average : ");
for(int i = 0; i < NUM_XEL; i++){
averBox[i] = (averBox[i] * (count - 1) + sampleAngleBox[i]) / (float)count;
printf("%.1f ", averBox[i]);
}
printf("\n");
}else{
printf("Move previous position.....");
arm.SetGoalPosition(getAngle);
WaitUntilMoveEnd(&arm);
printf("complete!\n");
}
}
//Deallocation
cv::destroyAllWindows();
motionHandler.Deinitialize();
kinectManager.Deinitialize();
printf("Go End position.....");
int Initpos[] = {0, 0, 0, 0, 0, 0, 2622, 1534, 2100};
arm.SetGoalPosition(Initpos);
WaitUntilMoveEnd(&arm);
printf("complete!\n");
arm.TorqueOff();
return 0;
}
object() : twister( initialize_twister( rand_dev() ) ) {}
