int main(int argc, char *argv[]) { double myJoints[ROBOT_DOF] = {1,2,3,4,5,6}; double jointsIn[ROBOT_DOF]; robotPose myPose, poseIn; int kukaConnection; RCS_TIMER *cycleBlock = new RCS_TIMER(KUKA_DEFAULT_CYCLE); TiXmlDocument kukaStatus; TiXmlHandle toSendHandle(&kukaStatus); TiXmlElement *cartesianStatus; TiXmlElement *cartesianUpdate; TiXmlElement *jointStatus; TiXmlElement *jointUpdate; TiXmlElement *IPOCUpdate; int nchars; TiXmlElement *cartesian; int debug = 0; int option; int counter = 0; double jointMotorScale[ROBOT_DOF], cmdMotorScale[ROBOT_DOF]; jointMotorScale[0] = 80.; jointMotorScale[1] = 100.; jointMotorScale[2] = 80.; jointMotorScale[3] = 80.; jointMotorScale[4] = 80.; jointMotorScale[5] = 40.5; cmdMotorScale[0] = 1.4; cmdMotorScale[1] = 1.74; cmdMotorScale[2] = 1.4; cmdMotorScale[3] = 1.4; cmdMotorScale[4] = 1.4; cmdMotorScale[5] = 0.87; while (true) { option = getopt(argc, argv, ":d"); if (option == -1) break; switch (option) { case 'd': debug = 1; break; case ':': fprintf(stderr, "missing value for -%c\n", optopt); return 1; break; default: fprintf (stderr, "unrecognized option -%c\n", optopt); return 1; break; } // switch (option) } // while (true) for getopt if( !kukaStatus.LoadFile(DEFAULT_FROM_KUKA)) { printf( "kukaRobot:: fatal error on load of %s\n", DEFAULT_FROM_KUKA); exit(1); } kukaConnection = ulapi_socket_get_client_id(KUKA_PORT, "localhost"); if( kukaConnection < 0 ) return -1; myPose.x = 0.1; myPose.y = 0.2; myPose.z = 0.3; myPose.xrot = 0.4; myPose.yrot = 0.5; myPose.zrot = 0.6; while(true) { TiXmlDocument kukaCorrections; TiXmlHandle correctionsHandle(&kukaCorrections); std::string str; enum {INBUF_LEN = 2048}; char inbuf[INBUF_LEN]; std::ostringstream s; cartesianStatus = toSendHandle.FirstChild("Rob").FirstChild("Dat").Child(1).ToElement(); IPOCUpdate = toSendHandle.FirstChild("Rob").FirstChild("Dat").Child(9). ToElement(); jointStatus = toSendHandle.FirstChild("Rob").FirstChild("Dat").Child(3).ToElement(); cartesianStatus->SetDoubleAttribute("X", myPose.x); cartesianStatus->SetDoubleAttribute ("Y", myPose.y); cartesianStatus->SetDoubleAttribute ("Z", myPose.z); cartesianStatus->SetDoubleAttribute ("A", myPose.xrot); cartesianStatus->SetDoubleAttribute ("B", myPose.yrot); cartesianStatus->SetDoubleAttribute ("C", myPose.zrot); jointStatus->SetDoubleAttribute("A1", myJoints[0]); jointStatus->SetDoubleAttribute("A2", myJoints[1]); jointStatus->SetDoubleAttribute("A3", myJoints[2]); jointStatus->SetDoubleAttribute("A4", myJoints[3]); jointStatus->SetDoubleAttribute("A5", myJoints[4]); jointStatus->SetDoubleAttribute("A6", myJoints[5]); s << counter++; s << '\0'; TiXmlText *text = new TiXmlText((s.str()).c_str()); IPOCUpdate->Clear(); IPOCUpdate->LinkEndChild(text); // kukaStatus.Print(); str << kukaStatus; ulapi_socket_write(kukaConnection, str.c_str(), str.length()); inbuf[0] = '\0'; nchars = ulapi_socket_read(kukaConnection, inbuf, sizeof(inbuf)-1); if (nchars <= 0) { printf("kukaRobot::status client disconnected\n"); break; } else { inbuf[nchars] = '\0'; } kukaCorrections.Parse(inbuf); if(debug) kukaCorrections.Print(); cartesianUpdate = correctionsHandle.FirstChild("Sen").FirstChild("Dat"). Child(1).ToElement(); jointUpdate = correctionsHandle.FirstChild("Sen").FirstChild("Dat"). Child(2).ToElement(); cartesianUpdate->QueryDoubleAttribute("X", &(poseIn.x)); cartesianUpdate->QueryDoubleAttribute("Y", &(poseIn.y)); cartesianUpdate->QueryDoubleAttribute("Z", &(poseIn.z)); cartesianUpdate->QueryDoubleAttribute("A", &(poseIn.xrot)); cartesianUpdate->QueryDoubleAttribute("B", &(poseIn.yrot)); cartesianUpdate->QueryDoubleAttribute("C", &(poseIn.zrot)); jointUpdate->QueryDoubleAttribute("A1", &(jointsIn[0])); jointUpdate->QueryDoubleAttribute("A2", &(jointsIn[1])); jointUpdate->QueryDoubleAttribute("A3", &(jointsIn[2])); jointUpdate->QueryDoubleAttribute("A4", &(jointsIn[3])); jointUpdate->QueryDoubleAttribute("A5", &(jointsIn[4])); jointUpdate->QueryDoubleAttribute("A6", &(jointsIn[5])); myPose.x += poseIn.x; myPose.y += poseIn.y; myPose.z += poseIn.z; myPose.xrot += poseIn.xrot; myPose.yrot += poseIn.yrot; myPose.zrot += poseIn.zrot; for( int i=0; i<ROBOT_DOF; i++ ) { myJoints[i] += jointsIn[i] * cmdMotorScale[i] / jointMotorScale[i]; if( debug ) printf( "J%d <%lf %lf> ", i+1, myJoints[i], jointsIn[i] ); } if(debug) printf( "\nkukaRobot Status: <%4.2f, %4.2f, %4.2f> <%4.2f, %4.2f, %4.2f>\n\n", myPose.x, myPose.y, myPose.z, myPose.xrot, myPose.yrot, myPose.zrot); cycleBlock->wait(); } }
rtapi_integer rtapi_socket_client(rtapi_integer port, const char *hostname) { return ulapi_socket_get_client_id(port, hostname); }
int main(int argc, char *argv[]) { enum {HOST_LEN = 256}; char host[HOST_LEN] = "localhost"; int state_port = STATE_PORT_DEFAULT; int cognex_port = COGNEX_PORT_DEFAULT; const double confidence = 0.95; double period = 1; int option; int ival; double dval; int state_client_id; ulapi_task_struct cognex_client_thread; cognex_object_info_db db; ulapi_task_struct *server_task; ulapi_integer retval; opterr = 0; while (true) { option = getopt(argc, argv, ":h:s:p:t:d"); if (option == -1) break; switch (option) { case 'h': strncpy(host, optarg, sizeof(host)); host[sizeof(host)-1] = 0; break; case 's': ival = atoi(optarg); state_port = ival; break; case 'p': ival = atoi(optarg); cognex_port = ival; break; case 't': dval = atof(optarg); period = dval; break; case 'd': debug = true; break; case ':': fprintf(stderr, "missing value for -%c\n", optopt); return 1; break; default: fprintf (stderr, "unrecognized option -%c\n", optopt); return 1; break; } // switch (option) } // while (true) for getopt // set up mapping of integer ids with object names object_names[1] = std::string("small_gear"); object_names[2] = std::string("medium_gear"); object_names[3] = std::string("large_gear"); object_names[4] = std::string("bottom_cover"); object_names[5] = std::string("top_cover"); // connect to state server state_client_id = ulapi_socket_get_client_id(state_port, host); if (state_client_id < 0) { fprintf(stderr, "can't connect to %s on port %d\n", host, state_port); return 1; } server_task = db.serve(cognex_port, period * 1e9); if (server_task == NULL) { fprintf(stderr, "can't serve port %d\n", cognex_port); return 1; } while (true) { enum {INBUF_LEN = 1024}; char inbuf[INBUF_LEN]; char *ptr; int nchars; int nleft; int length; int message_type; object_state_message obj_state; nchars = ulapi_socket_read(state_client_id, inbuf, sizeof(inbuf)); if (nchars <= 0) break; inbuf[sizeof(inbuf)-1] = 0; ptr = inbuf; nleft = nchars; while (nleft > 0) { // get the length and type of the message memcpy(&length, ptr, sizeof(length)); memcpy(&message_type, ptr + sizeof(length), sizeof(message_type)); // switch on the message type and handle it switch (message_type) { case MESSAGE_ROBOT_STATUS: case MESSAGE_JOINT_POSITION: break; case MESSAGE_OBJECT_STATE: if (0 == obj_state.read_object_state(ptr)) { if (debug) obj_state.print_object_state(); for (int t = 0; t < obj_state.number; t++) { db.add(obj_state.objects[t].id, cognex_object_info(object_names[obj_state.objects[t].id].c_str(), 2*atan2(obj_state.objects[t].qz, obj_state.objects[t].qw), obj_state.objects[t].x, obj_state.objects[t].y, confidence)); } } break; default: // unknown message break; } // switch (message type) nleft -= (sizeof(length) + length); ptr += (sizeof(length) + length); } // while (nleft) } // while (true) ulapi_task_join(server_task, &retval); return 0; }
int main(int argc, char *argv[]) { int option; ulapi_integer port = SAMPLE_APP_DEFAULT_PORT; enum {BUFFERLEN = 256}; char host[BUFFERLEN] = "localhost"; ulapi_integer debug = 0; ulapi_integer client_id; ulapi_task_struct *client_task; client_args *client_args_ptr; client_db_struct client_db; char buffer[BUFFERLEN]; char *ptr; int number; ulapi_opterr = 0; for (;;) { option = ulapi_getopt(argc, argv, ":p:h:d"); if (option == -1) break; switch (option) { case 'p': port = atoi(ulapi_optarg); break; case 'h': strncpy(host, ulapi_optarg, sizeof(host)); host[sizeof(host) - 1] = 0; break; case 'd': debug = 1; break; case ':': fprintf(stderr, "missing value for -%c\n", ulapi_optopt); return 1; break; default: /* '?' */ fprintf(stderr, "unrecognized option -%c\n", ulapi_optopt); return 1; break; } } if (ulapi_optind < argc) { fprintf(stderr, "extra non-option characters: %s\n", argv[ulapi_optind]); return 1; } if (ULAPI_OK != ulapi_init()) { fprintf(stderr, "ulapi_init error\n"); return 1; } if (debug) ulapi_set_debug(ULAPI_DEBUG_ALL); if (0 != sample_app_init()) { fprintf(stderr, "can't init the sample app\n"); return 1; } client_id = ulapi_socket_get_client_id(port, host); if (client_id < 0) { fprintf(stderr, "can't connect to port %d\n", (int) port); ulapi_exit(); return 1; } if (debug) { printf("serving port %d\n", (int) port); } client_db.mutex = ulapi_mutex_new(0); client_db.number = 0; client_task = ulapi_task_new(); client_args_ptr = reinterpret_cast<client_args *>(malloc(sizeof(client_args))); client_args_ptr->client_task = client_task; client_args_ptr->client_id = client_id; client_args_ptr->client_db_ptr = &client_db; client_args_ptr->debug = debug; ulapi_task_start(client_task, client_code, client_args_ptr, ulapi_prio_lowest(), 0); /* enter application main loop */ while (!feof(stdin)) { if (NULL == fgets(buffer, sizeof(buffer), stdin)) { break; } ptr = buffer; while (isspace(*ptr)) ptr++; if ('q' == *ptr) break; if (0 == *ptr) { ulapi_mutex_take(client_db.mutex); number = client_db.number; ulapi_mutex_give(client_db.mutex); printf("%d\n", number); continue; } if (1 == sscanf(ptr, "%d", &number)) { ulapi_mutex_take(client_db.mutex); client_db.number = number; ulapi_mutex_give(client_db.mutex); continue; } } ulapi_socket_close(client_id); ulapi_exit(); sample_app_exit(); return 0; }
int main(int argc, char *argv[]) { bool done; int option; int cmdConnection; int script = 1; char msgOut[buffSize]; RCS_TIMER *cycleBlock = new RCS_TIMER(KUKA_DEFAULT_CYCLE); CRCLStatus status; ulapi_task_struct statusTask; RobotStatus robotStatus; GripperStatus gripperStatus; bool useFile = false; char *fileName; std::ifstream inFile; std::istream *inputSource; float counter = 0; debug = 1; while (true) { option = getopt(argc, argv, ":cdf:"); if (option == -1) break; switch (option) { case 'd': debug = 1; break; case 'c': script = 0; break; case 'f': script = 0; useFile = true; fileName = optarg; break; case ':': fprintf(stderr, "missing value for -%c\n", optopt); return 1; break; default: fprintf (stderr, "unrecognized option -%c\n", optopt); return 1; break; } // switch (option) } // while (true) for getopt // this code uses the ULAPI library to provide portability // between different operating systems and architectures if (ULAPI_OK != ulapi_init()) { printf ("crclClient:: can't initialize ulapi"); return 1; } cmdConnection = ulapi_socket_get_client_id(CRCL_CMD_PORT_DEFAULT, "localhost"); if(cmdConnection < 0) return -1; // start status thread ulapi_task_init(&statusTask); ulapi_task_start(&statusTask, reinterpret_cast<ulapi_task_code>(statusThread), reinterpret_cast<void*>(&status), ulapi_prio_lowest(), 0); if(!script) { if( useFile ) { inFile.open(fileName); inputSource = &inFile; if( !inputSource->good() ) { printf( "unable to open %s\n", fileName ); exit(1); } } else inputSource = &std::cin; while(inputSource->good()) { inputSource->getline(msgOut, buffSize); if( msgOut[0] == ' ' || msgOut[0] == '/' ) continue; ulapi_socket_write(cmdConnection, msgOut, strlen(msgOut)); printf( "Sent: %s\n", msgOut); done = false; if(useFile) { for(int ii=0; ii<5; ii++) cycleBlock->wait(); } while( !done ) { cycleBlock->wait(); gripperStatus = status.getGripperStatus(); robotStatus = status.getRobotStatus(); if( !useFile || status.getCurrentStatus()==CRCL_DONE ) done = true; // printf( "Fmod: %f\n", fmod(counter, 100.)); if( fmod(counter++, 100.)==0 || done ) { printf( "Status Cmd: %s Status: %s\n", getCRCLCmdString((status.getCurrentCmd()).cmd).c_str(), getCRCLStatusString(status.getCurrentStatus()).c_str()); printf( "\x1b[32mCart Status: <%3.4f, %3.4f, %3.4f> <%3.4f, %3.4f, %3.4f>\x1b[0m\n", robotStatus.pose.x, robotStatus.pose.y, robotStatus.pose.z, robotStatus.pose.xrot, robotStatus.pose.yrot, robotStatus.pose.zrot); printf( "\x1b[32mJoint Status: <%4.2f %4.2f %4.2f %4.2f %4.2f %4.2f>\x1b[0m\n", robotStatus.joint[0], robotStatus.joint[1], robotStatus.joint[2], robotStatus.joint[3], robotStatus.joint[4], robotStatus.joint[5]); } } } } for(int i=0; i<1; i++ ) { ulapi_strncpy(msgOut, "InitCanon", strlen("InitCanon")); ulapi_socket_write(cmdConnection, msgOut, strlen(msgOut)); /* printf( "crclClient writing: %s\n", msgOut.c_str()); sleep(2); msgOut = "MoveTo 1. 2. 3. 4. 5. 6."; ulapi_socket_write(cmdConnection, msgOut.c_str(), msgOut.length()); printf( "crclClient writing: %s\n", msgOut.c_str()); sleep(2); msgOut = "Dwell 1"; ulapi_socket_write(cmdConnection, msgOut.c_str(), msgOut.length()); printf( "crclClient writing: %s\n", msgOut.c_str()); sleep(2); msgOut = "MoveTo 7. 8. 9. 10. 11. 12."; ulapi_socket_write(cmdConnection, msgOut.c_str(), msgOut.length()); printf( "crclClient writing: %s\n", msgOut.c_str()); sleep(2); msgOut = "Dwell 4"; ulapi_socket_write(cmdConnection, msgOut.c_str(), msgOut.length()); printf( "crclClient writing: %s\n", msgOut.c_str()); sleep(2); msgOut = "EndCanon"; ulapi_socket_write(cmdConnection, msgOut.c_str(), msgOut.length()); printf( "crclClient writing: %s\n", msgOut.c_str()); sleep(2); */ cycleBlock->wait(); } }