int main(int argc, char** argv)
{
// Init the ROS node
ros::init(argc, argv, "robot_driver");
RobotArm arm;
// Start the trajectory. This will not return until completion.
arm.startTrajectory(arm.armExtensionTrajectory());
}
int main(int argc, char** argv)
{
// Init the ROS node
ros::init(argc, argv, "robot_arm_driver_ltl");
sleep(40.0);
RobotArm arm;
// Start the trajectory
//arm.startTrajectory(//* *//);
// Wait for trajectory completion
while(!arm.getState().isDone() && ros::ok())
{
usleep(50000);
}
}
int main(int argc, char** argv)
{
// Init the ROS node
ros::init(argc, argv, "robot_driver");
RobotArm arm;
// Start the trajectory
arm.startTrajectory(arm.armExtensionTrajectory());
// Wait for trajectory completion
while(!arm.getState().isDone() && ros::ok())
{
usleep(50000);
}
}
int main(int argc, char** argv)
{
// Init the ROS node
ros::init(argc, argv, "robot_driver");
if (argc == 2) /// If the program was started with a trajectory parameter to set in an initial position the robo
{
std::string traj_filename(argv[1]);
//std::string traj_filename("/home/robot/code/iri/libbarrett/bin/test");
RobotArm arm;
// IMPORTANT: The robot must be at the start trajectory position
//arm.startJointsMove(arm.armAtHomePosition());
// Start the trajectory
control_msgs::FollowJointTrajectoryGoal trajectory_goal = arm.getProMpTrajectory(traj_filename);
arm.startJointsMove(arm.getJointsMovementSrvFromPoint(trajectory_goal.trajectory.points[0]));
arm.startTrajectory(trajectory_goal);
ROS_INFO("Action finished: %s\n", arm.getState().toString().c_str());
// Wait for trajectory completion
while(!arm.getState().isDone() && ros::ok())
{
usleep(50000);
}
}else{
std::cerr << "You must pass the path to the stored trajectory as an argument" << std::endl;
std::cerr << "Example rosrun iri_wam_controller test_simple_trajectory_promp /home/user/path/to/file" << std::endl;
return 0;
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "r_arm_test");
RobotArm arm;
arm.startTrajectory(arm.armExtensionTrajectory(), arm.torsoExtensionTrajectory());
// Wait for trajectory completion
while (!arm.getArmState().isDone() && !arm.getTorsoState().isDone() && ros::ok())
{
usleep(50000);
}
}
/** Construct the scene */
Scene() :
// You have to call the parent class's constructor, to provide a
// name for the model.
Model("Scene"),
worm1("1"),
worm2("2"),
ground(),
mushroom1("1"),
mushroom2("2"),
// Construct textures and shaders.
// They won't be loaded until the model is drawn for the first time.
texture("checkers.png"),
shader("shader.vert", "shader.frag", NULL),
// Call the constructors for the lights
pointLight("Point Light", GL_LIGHT1, /**direction part**/ -5, 5, 5, /**diffuse part**/ 1.0, 0.5, 0.5,
/**specular part**/ 1.0, 0.5, 0.5, /**ambient part**/ .2f, 0.1, 0.1 /**attenuation part**/, 0.4, 0.7, 0),
directionalLight("Directional Light", GL_LIGHT0, /**direction part**/ 5, 5, 5, /**diffuse part**/ 0.0f, 0.5, 0.5f,
/**specular part**/ 0.0f, 0.5f, 0.5f )
// Now, call the constructors for each Property:
{
// If you have child Models, like the MobileLight model from model.h,
// you can add their property groups, and they will appear in the list
// in the top left corner of Modeler, under this model's entry:
properties.add(pointLight.getProperties())
.add(directionalLight.getProperties());
properties.add(robotArm.getProperties())
.add(worm1.getProperties())
.add(worm2.getProperties())
.add(ground.getProperties())
.add(mushroom1.getProperties())
.add(mushroom2.getProperties());
// Finally, add all the properties to this model's PropertyGroup.
}
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;
}
int main(void) {
try {
initscr();
timeout(-1);
printw("Robot arm interface by George Higgins-Smith\n");
printw("Wrist commands: i, o, p\n\r");
printw("Base commands: k, l, ;\n\r");
printw("Grip commands: w, s, x\n\r");
printw("Elbow commands: e, d, c\n\r");
printw("Shoulder commands: r, f, v\n\r");
printw("Toggle LED: space\n\r");
printw("Quit: q\n\r");
RobotArm roboarm;
roboarm.connect();
bool LED = false;
bool running = true;
while(running) {
int key = getch();
switch(key) {
// wrist
case 'i': roboarm.setGrip(RobotArm::ARM_OPEN); break;
case 'o': roboarm.setGrip(RobotArm::ARM_STOP); break;
case 'p': roboarm.setGrip(RobotArm::ARM_CLOSE); break;
// base
case 'k': roboarm.setBase(RobotArm::ARM_ANTICLOCKWISE); break;
case 'l': roboarm.setBase(RobotArm::ARM_STOP); break;
case ';': roboarm.setBase(RobotArm::ARM_CLOCKWISE); break;
// grip
case 'w': roboarm.setWrist(RobotArm::ARM_UP); break;
case 's': roboarm.setWrist(RobotArm::ARM_STOP); break;
case 'x': roboarm.setWrist(RobotArm::ARM_DOWN); break;
// elbow
case 'e': roboarm.setElbow(RobotArm::ARM_UP); break;
case 'd': roboarm.setElbow(RobotArm::ARM_STOP); break;
case 'c': roboarm.setElbow(RobotArm::ARM_DOWN); break;
// shoulder
case 'r': roboarm.setShoulder(RobotArm::ARM_UP); break;
case 'f': roboarm.setShoulder(RobotArm::ARM_STOP); break;
case 'v': roboarm.setShoulder(RobotArm::ARM_DOWN); break;
// LED
case ' ': roboarm.setLED(LED = !LED); break;
// quit
case 'q': roboarm.stopAll(); running = false; break;
}
}
roboarm.stopAll();
roboarm.disconnect();
endwin();
} catch(const char * message) {
endwin();
printf("%s\n\r", message);
return(1);
}
printf("Find out more at https://github.com/bu1l3r/robo-arm\n");
return 0;
}
int main(int argc, char** argv){
ros::init(argc, argv, "lift_test");
// figure out what kind of five we want
bool left = false;
bool right = true;
if(argc > 1)
{
left = (strcmp(argv[1],"left") == 0) || (strcmp(argv[1],"double") == 0);
right = (strcmp(argv[1],"right") == 0) || (strcmp(argv[1],"double") == 0);
}
RobotArm arm;
Gripper gripper;
gripper.close(left,right,true);
while(ros::ok())
{
float pre_five_r []= {-1.4204039529994779, 0.64014688694872024, 0.64722849826846929, -1.9254939970572147, 30.931365914354672, -0.52166442520665446, -16.642483924162743 };
float pre_five_l []= {1.544791237364544, 0.028669297805660576, -0.061946460502633194, -1.9322034349027488, -0.96915535302752587, -0.22688029069077575, -3.5411348633607669};
if(left)
arm.startTrajectory(arm.arm_trajectoryPoint(pre_five_l,2.0,false),false);
if(right)
arm.startTrajectory(arm.arm_trajectoryPoint(pre_five_r,2.0,true),true);
sleep(2.0);
float five_r []= {-0.43912122996219438, -0.26865637196243625, -0.06073806015457861, -1.0597651429837187, 30.217764249732564, -0.098888248598895112, -17.139399300620212 };
float five_l []= {0.34795130919909756, -0.33483508677418122, 0.21450526015905091, -0.87161320330702452, -0.55300340950519367, -0.51138511922202357, -3.1737890509598912};
if(left)
arm.startTrajectory(arm.arm_trajectoryPoint(five_l,1.25,false),false);
if(right)
arm.startTrajectory(arm.arm_trajectoryPoint(five_r,1.25,true),true);
// now start looking for a slap during the move
gripper.slap(left,right);
//wait for a slap
while(!gripper.slapDone(left,right) && ros::ok())
{
ros::Duration(0.005).sleep();
}
float post_five_r []= {-1.2271486279403441, 0.98994689398564029, 0.27937452657595019, -1.9855738422813785, 31.095246711685615, -0.75469820126008202, -17.206098475132887 };
float post_five_l []= {1.458651261930636, 1.0444005395208478, 0.081571109098415029, -2.1115743463069396, -1.3390296269894097, -0.16823026639652239, -3.5006715676681437};
if(left)
arm.startTrajectory(arm.arm_trajectoryPoint(post_five_l,1.35,false),false);
if(right)
arm.startTrajectory(arm.arm_trajectoryPoint(post_five_r,1.35,true),true);
sleep(2.0);
}
return 0;
}
