void test_particle_update() { Vector3d object(3.35, -1.11, 0.8); Arm::ArmType arm_type = Arm::ArmType::right; bool view = true; PR2System sys(object, arm_type, view); PR2* brett = sys.get_brett(); Arm* arm = sys.get_arm(); rave::EnvironmentBasePtr env = brett->get_env(); sleep(2); arm->set_posture(Arm::Posture::mantis); // setup scenario VectorJ j_t_real = arm->get_joint_values(), j_tp1_real; VectorJ j_t = j_t_real, j_tp1; // i.e. no belief == actual VectorU u_t = VectorU::Zero(); MatrixP P_t = setup_particles(env), P_tp1; LOG_INFO("Origin particles"); std::vector<ParticleGaussian> particle_gmm_t; sys.fit_gaussians_to_pf(P_t, particle_gmm_t); sys.display(j_t, particle_gmm_t); sys.execute_control_step(j_t_real, j_t, u_t, P_t, j_tp1_real, j_tp1, P_tp1); LOG_INFO("New particles") std::vector<ParticleGaussian> particle_gmm_tp1; sys.fit_gaussians_to_pf(P_tp1, particle_gmm_tp1); sys.display(j_tp1, particle_gmm_tp1); }
void GamepadArmControl(Gamepad *gp) { // arm shoulder control if ( gp->GetButton(Gamepad::kBottomButton) ) _arm->lowerShoulder(); else if ( gp->GetButton(Gamepad::kTopButton) ) _arm->raiseShoulder(); // claw control w/top right trigger if ( gp->GetButton(Gamepad::kRightTopTrigger) ) _claw->open(); else _claw->close(); double stick = gp->GetLeftY(); // FIXME: is the minus sign necessary???? SmartDashboard::Log(stick, "Elbow Control Stick"); double elbowPower = stick * -ELBOW_MAX_POWER; _arm->_elbowController->Set(elbowPower); }
void test_pr2_system() { Vector3d object(3.35, -1.11, 0.8); Arm::ArmType arm_type = Arm::ArmType::right; bool view = true; PR2System sys(object, arm_type, view); PR2* brett = sys.get_brett(); Arm* arm = sys.get_arm(); rave::EnvironmentBasePtr env = brett->get_env(); sleep(2); arm->set_posture(Arm::Posture::mantis); // setup particles MatrixP P = setup_particles(env); // test plotting VectorJ j = arm->get_joint_values(); std::vector<ParticleGaussian> particle_gmm; particle_gmm.push_back(ParticleGaussian(Vector3d::Zero(), Matrix3d::Identity(), P.leftCols(M_DIM/2), 1)); particle_gmm.push_back(ParticleGaussian(Vector3d::Zero(), Matrix3d::Identity(), P.rightCols(M_DIM/2), 1)); sys.display(j, particle_gmm); }
void test_fk() { Vector3d object(3.35, -1.11, 0.8); Arm::ArmType arm_type = Arm::ArmType::right; bool view = true; PR2System sys(object, arm_type, view); PR2* brett = sys.get_brett(); Arm* arm = sys.get_arm(); Camera* cam = sys.get_camera(); rave::EnvironmentBasePtr env = brett->get_env(); arm->set_posture(Arm::Posture::mantis); sleep(1); VectorJ j = arm->get_joint_values(); Matrix4d actual_arm_pose = rave_utils::transform_from_to(brett->get_robot(), Matrix4d::Identity(), "r_gripper_tool_frame", "world"); Matrix4d fk_arm_pose = arm->get_pose(j); std::cout << "actual_arm_pose:\n" << actual_arm_pose << "\n\n"; std::cout << "fk_arm_pose:\n" << fk_arm_pose << "\n\n"; Matrix4d actual_cam_pose = rave_utils::rave_to_eigen(cam->get_sensor()->GetTransform()); Matrix4d fk_cam_pose = cam->get_pose(j); std::cout << "actual_cam_pose:\n" << actual_cam_pose << "\n\n"; std::cout << "fk_cam_pose:\n" << fk_cam_pose << "\n\n"; }
int main(int argc, char *argv[]) { signal(SIGINT, stopsignal); // Connect to the arm start_arm(); sleep(1); vec values = arm.sense(); arm.set_pose(values(0), values(1), values(2), values(3), values(4), values(5)); sense = values; values += vec({ 90, 90, 0, 90, 90, 0 }); iValue[0] = (values(0)); iValue[1] = (values(1)); iValue[2] = (values(2)); iValue[3] = (values(3)); iValue[4] = (values(4)); iValue[5] = (values(5)); // Run loop thread ui_thread(run_ui); ui_thread.join(); // wait until the ui is finished for (int i = 0; i < 100; i++) { arm.set_pose(0, 0, 0, 0, 0, 0, false); } stop_arm(); return 0; }
void update2(double value) { //goal = Point3f(cos(angle*PI / 180.0f), sin(angle*PI / 180.0f), (sin(zangle*PI / 180.0f))); //Point3f goal(cos(angle*PI/180.0f), sin(angle*PI/180.0f), 0); //goal.normalize(); //goal *= 5; //goal += Vector3f(-3, 0, -1); //goal = Vector3f(0, 0, 7); //mainArm.solve(goal, 100); //Point3f secGoal = Point3f(goal[0], goal[1], -goal[2]); //secArm.solve(secGoal, 100); //display(); for (int i = 0; i < 50; i++) { if (i>20) { goal = Point3f(3, 0, 1); //goal += Vector3f(20, 4, 4); mainArm.solve(goal, 100); display(); } } for (int i = 0; i < 50;i++){ if(i>25){ goal = Vector3f(value+3, 0, 3); mainArm.solve(goal, 100); display(); } } }
void test_figtree() { Vector3d object(3.35, -1.11, 0.8); Arm::ArmType arm_type = Arm::ArmType::right; bool view = true; PR2System sys(object, arm_type, view); PR2* brett = sys.get_brett(); Arm* arm = sys.get_arm(); rave::EnvironmentBasePtr env = brett->get_env(); arm->set_posture(Arm::Posture::mantis); sleep(1); MatrixP P = setup_particles(env); std::vector<ParticleGaussian> particle_gmm; tc.start("figtree"); sys.fit_gaussians_to_pf(P, particle_gmm); tc.stop("figtree"); std::cout << "particle_gmm size: " << particle_gmm.size() << "\n"; for(int i=0; i < particle_gmm.size(); ++i) { std::cout << "mean: " << particle_gmm[i].mean.transpose() << "\n"; std::cout << "cov diag: " << particle_gmm[i].cov.diagonal().transpose() << "\n"; std::cout << "pct: " << particle_gmm[i].pct << "\n"; } tc.print_all_elapsed(); sys.display(arm->get_joint_values(), particle_gmm); }
/* ************************************************************************** */ Pose2MobileArm::Pose2MobileArm(const Arm& arm, const gtsam::Pose3& base_T_arm) : Base(arm.dof() + 3, arm.dof() + 1), base_T_arm_(base_T_arm), arm_(arm) { // check arm base pose, warn if non-zero if (!arm_.base_pose().equals(Pose3::identity(), 1e-6)) cout << "[Pose2MobileArm] WARNING: Arm has non-zero base pose, this base pose will be override. " "Set the base_T_arm in Pose2MobileArm." << endl; }
/* Move arm to various places */ void test_move(Arm &arm) { arm.move_to( iPair(4,2) ); arm.move_to( iPair(3,0) ); arm.move_to( iPair(2,2) ); arm.move_to( iPair(0,0) ); }
/* ************************************************************************** */ Pose2MobileVetLinArm::Pose2MobileVetLinArm(const Arm& arm, const gtsam::Pose3& base_T_torso, const gtsam::Pose3& torso_T_arm, bool reverse_linact) : Base(arm.dof() + 4, arm.dof() + 2), base_T_torso_(base_T_torso), torso_T_arm_(torso_T_arm), reverse_linact_(reverse_linact), arm_(arm) { // check arm base pose, warn if non-zero if (!arm_.base_pose().equals(Pose3::identity(), 1e-6)) cout << "[Pose2MobileArm] WARNING: Arm has non-zero base pose, this base pose will be override. " "Set the base_T_torso and torso_T_arm in Pose2MobileArm." << endl; }
bool mtsIntuitiveResearchKitConsole::AddArm(mtsComponent * genericArm, const mtsIntuitiveResearchKitConsole::Arm::ArmType CMN_UNUSED(armType)) { // create new required interfaces to communicate with the components we created Arm * newArm = new Arm(genericArm->GetName(), ""); if (SetupAndConnectInterfaces(newArm)) { mArms.push_back(newArm); return true; } CMN_LOG_CLASS_INIT_ERROR << GetName() << ": AddArm, unable to add new arm. Are you adding two arms with the same name? " << newArm->Name() << std::endl; delete newArm; return false; }
void test_board_move(Arm &arm) { Move move(iPair(4,1), iPair(4, 2)); /* Move arm according to move */ arm.execute_move(move); /* Move arm to 0,0) */ arm.move_to(iPair(0,0)); /* move = Move(iPair(5, 0), iPair(4,0)); */ /* arm.execute_move(move); */ /* arm.move_to( iPair(3,0) ); */ /* arm.move_to(iPair(0,0) ); */ }
void dropInBox(int place) { arm.gotoPlace(place); needle.down(); delay(500); needle.up(); }
void run_ui(void) { string winname = "Press 'q' to quit"; namedWindow(winname); createTrackbar("Joint0", winname, &iValue[0], 180, joint0callback, NULL); createTrackbar("Joint1", winname, &iValue[1], 180, joint1callback, NULL); createTrackbar("Joint2", winname, &iValue[2], 180, joint2callback, NULL); createTrackbar("Joint3", winname, &iValue[3], 180, joint3callback, NULL); createTrackbar("Joint4", winname, &iValue[4], 180, joint4callback, NULL); createTrackbar("Claw", winname, &iValue[5], 180, joint5callback, NULL); cv::Mat bufimage(480, 720, CV_8UC3); char numbuf[64]; for (;;) { bufimage = Scalar(255, 255, 255); vec values = sense + vec({ 90, 90, 0, 90, 90, 0 }); // Display the value bars for (int i = 0; i < (int)values.n_elem; i++) { Point topleft(80, 20 + i * 80); Point btmright(values(i) + 80, 60 + i * 80); rectangle(bufimage, topleft, btmright, Scalar(255, 32, 10)); sprintf(numbuf, "%0.2lf", sense(i)); putText(bufimage, string(numbuf), Point(20, 60 + i * 80), FONT_HERSHEY_PLAIN, 1, Scalar(255, 32, 10), 1, 8, false); } // Display the forward kinematics arm.arm_read = sense; mat positions(3, 8, fill::zeros); for (int i = 0; i <= 6; i++) { vec eepos = arm.get_end_effector_pos(i); positions.col(i+1) = eepos; sprintf(numbuf, "[%0.2lf %0.2lf %0.2lf]", eepos(0), eepos(1), eepos(2)); putText(bufimage, string(numbuf), Point(280, 60 + i * 60), FONT_HERSHEY_PLAIN, 1, Scalar(255, 32, 10), 1, 8, false); } // Draw shit vector<Scalar> colors = { Scalar(0,0,0), Scalar(255,0,128), Scalar(255,0,0), Scalar(255,128,0), Scalar(0,128,0), Scalar(0,128,255), Scalar(0,0,255) }; for (int i = 0; i <= 6; i++) { vec pos1 = positions.col(i); pos1 = vec({ pos1(1), -pos1(2) }); pos1 = 4 * pos1 + vec({ 560, 150 }); vec pos2 = positions.col(i+1); pos2 = vec({ pos2(1), -pos2(2) }); pos2 = 4 * pos2 + vec({ 560, 150 }); line(bufimage, Point(pos1(0), pos1(1)), Point(pos2(0), pos2(1)), colors[i], 2); } imshow(winname, bufimage); int k = waitKey(30); if ((k & 0x7f) == 'q') { break; } } }
bool mtsIntuitiveResearchKitConsole::AddArm(mtsComponent * genericArm, const mtsIntuitiveResearchKitConsole::Arm::ArmType CMN_UNUSED(armType)) { // create new required interfaces to communicate with the components we created Arm * newArm = new Arm(genericArm->GetName(), ""); if (AddArmInterfaces(newArm)) { ArmList::iterator armIterator = mArms.find(newArm->mName); if (armIterator != mArms.end()) { mArms[newArm->mName] = newArm; return true; } } CMN_LOG_CLASS_INIT_ERROR << GetName() << ": AddArm, unable to add new arm. Are you adding two arms with the same name? " << newArm->Name() << std::endl; delete newArm; return false; }
~Sling() { arm.move_to( iPair(0,0) ); mav(motor_shoulder, -200); mav(motor_arm, -200); sleep(0.2); ao(); }
bool ARM_module::initialize() { if(this->isInitialized() == true) return 1; else return arm.initialize(); }
void pickup() { arm.gotoPickup(); needle.middle(); delay(2000); needle.up(); }
void update() { angle += 360.0 / 60.0; zangle += 360.0f / 100.0f; //goal = Point3f(cos(angle*PI / 180.0f), sin(angle*PI / 180.0f), (sin(zangle*PI / 180.0f))); //Point3f goal(cos(angle*PI/180.0f), sin(angle*PI/180.0f), 0); //goal.normalize(); //goal *= 5; //goal += Vector3f(-3, 0, -1); //goal = Vector3f(0, 0, 7); //mainArm.solve(goal, 100); //Point3f secGoal = Point3f(goal[0], goal[1], -goal[2]); //secArm.solve(secGoal, 100); //display(); goal = Vector3f(20, 4, 4); mainArm.solve(goal, 100); display(); }
int main() { cout << "Begin" << endl; //test_sensors(); /* Arm back to 0,0 position */ Arm arm; arm.reset(); /* Move arm */ test_board_move(arm); //test_move(arm); /* Arm back to 0,0 position */ arm.reset(); cout << "Done." << endl; return 0; }
void LogToDashboard() { _arm->logInfo(); _lightSensors->logInfo(); // minibot deployment SmartDashboard::Log(MinibotDeployable(), "Minibot Deployable"); SmartDashboard::Log(_minibotDeployerSolenoid->Get(), "Minibot Deployed"); }
void display() { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // clear the color buffer // set camera parameters GLdouble centerX = eyeX; GLdouble centerY = eyeY + 1; GLdouble centerZ = eyeZ; GLdouble upX = 0.; GLdouble upY = 0.; GLdouble upZ = 1.; glMatrixMode(GL_MODELVIEW); // indicate we are specifying camera transformations glLoadIdentity(); gluLookAt(eyeX, eyeY, eyeZ, centerX, centerY, centerZ, upX, upY, upZ); glMatrixMode(GL_PROJECTION); glLoadIdentity(); gluPerspective(60, 1, 1, 100); // drawing is done here mainArm.draw(); // secArm.draw(); float c = 0.2; Point3f a0 = goal + Vector3f(-c, 0, c); Point3f a1 = goal + Vector3f(0, 0, -c); Point3f a2 = goal + Vector3f(c, 0, c); Vector3f n2(0, -1, 0); glBegin(GL_TRIANGLES); glNormal3f(n2[0], n2[1], n2[2]); glVertex3f(a0[0], a0[1], a0[2]); glVertex3f(a1[0], a1[1], a1[2]); glVertex3f(a2[0], a2[1], a2[2]); glEnd(); /*Point3f secGoal = Point3f(goal[0], goal[1], -goal[2]); a0 = secGoal + Vector3f(-c, 0, c); a1 = secGoal + Vector3f(0, 0, -c); a2 = secGoal + Vector3f(c, 0, c); glBegin(GL_TRIANGLES); glNormal3f(n2[0], n2[1], n2[2]); glVertex3f(a0[0], a0[1], a0[2]); glVertex3f(a1[0], a1[1], a1[2]); glVertex3f(a2[0], a2[1], a2[2]);*/ glEnd(); // end drawing glFlush(); glutSwapBuffers(); // swap buffers (we earlier set double buffer) }
bool AngularMotor::moveArm(side s,Arm A){ if( s == right_side ){ effect << " (" << "rae1 " << A.getaj1() << ") "; effect << " (" << "rae2 " << A.getaj2() << ") "; effect << " (" << "rae3 " << A.getaj3() << ") "; effect << " (" << "rae4 " << A.getaj4() << ") "; }else if ( s == left_side ){ effect << " (" << "lae1 " << A.getaj1() << ") "; effect << " (" << "lae2 " << A.getaj2() << ") "; effect << " (" << "lae3 " << A.getaj3() << ") "; effect << " (" << "lae4 " << A.getaj4() << ") "; } return true; }
void AddArm() { cin.get(); cout << endl; cout << "********************************************************" << endl; cout << "* Add ARM vip *" << endl; cout << "********************************************************" << endl; cout << endl; try { Arm* newArm = new Arm(); if(!newArm) throw new Except("Add ARM vip failed!"); newArm->Init(); cin.get(); vip.push_back(newArm); } catch (Except* pEx) { cout<<endl; cout << "********************************************************" << endl; cout << "* It appear error as below : *" << endl; cout << "* pEx->what() *" << endl; cout << "********************************************************" << endl; cout<<endl; } cout << endl; cout << "****************************************************" << endl; cout << "* Press ENTER to back to menu *" << endl; cout << "****************************************************" << endl; cin.get(); }
/** * This function runs in the control thread, and continually sets the motors to the correct speed. * The speed is determined by the difference in angle divided by a constant. */ void Arm::control_arm_motor( void *object ) { Arm *instance = (Arm *) object; instance->upperArmAngle = instance->GetTilt(); // Runs when the difference in angles is large enough //TODO: fix kCloseEnough with real value while ( fabs( instance->upperArmAngle - instance->arm_control_angle ) > kCloseEnough ) { instance->upperArmAngle = instance->GetTilt(); if (((instance->upperArmAngle-instance->arm_control_angle ) / 700 < 0.5) && ((instance->upperArmAngle-instance->arm_control_angle ) / 2000 > 0) ) { instance->armMotor.Set(0.5); } else if (((instance->upperArmAngle-instance->arm_control_angle ) / 700 > -0.5) && ((instance->upperArmAngle-instance->arm_control_angle ) / 2000 < 0) ) { instance->armMotor.Set(-0.5); } else if ((instance->upperArmAngle-instance->arm_control_angle ) / 700 >= 1.0 ) { instance->armMotor.Set(1.0); } else if ((instance->upperArmAngle-instance->arm_control_angle ) / 700 <= -1.0 ) { instance->armMotor.Set(-1.0); } else { instance->armMotor.Set( ( instance->upperArmAngle-instance->arm_control_angle ) / 700 ); } Wait( 0.05 ); } instance->armMotor.Set( 0.0 ); }
void start_arm() { arm.connect(); if (!arm.connected()) { printf("[ARM TEST] Not connected to anything, disconnecting...\n"); arm.disconnect(); exit(1); } arm.load_calibration_params("calib_params.json"); if (!arm.calibrated()) { arm.disconnect(); exit(1); } }
int checkColor() { arm.gotoCheck(); delay(3000); return Arm::Red; }
void setup() { arm.init(); needle.init(); }
//-------------------------------------------------------------- void testApp::recordArmXML (Hand & hand){ if (bRecordingThisFrame){ Arm arm = hand.arm(); if (arm.isValid()){ float armWidth = arm.width(); XML.setValue("AW", armWidth, lastTagNumber); ofPoint palmPt = leap.getofPoint ( hand.palmPosition()); ofPoint wristPt = leap.getofPoint ( arm.wristPosition()); ofPoint elbowPt = leap.getofPoint ( arm.elbowPosition()); ofPoint palmNorm = leap.getofPoint ( hand.palmNormal()); int palmPtTagNum = XML.addTag("PM"); XML.setValue("PM:X", palmPt.x, palmPtTagNum); XML.setValue("PM:Y", palmPt.y, palmPtTagNum); XML.setValue("PM:Z", palmPt.z, palmPtTagNum); int wristPtTagNum = XML.addTag("W"); XML.setValue("W:X", wristPt.x, wristPtTagNum); XML.setValue("W:Y", wristPt.y, wristPtTagNum); XML.setValue("W:Z", wristPt.z, wristPtTagNum); int elbowPtTagNum = XML.addTag("E"); XML.setValue("E:X", elbowPt.x, elbowPtTagNum); XML.setValue("E:Y", elbowPt.y, elbowPtTagNum); XML.setValue("E:Z", elbowPt.z, elbowPtTagNum); int palmNormTagNum = XML.addTag("PN"); XML.setValue("PN:X", palmNorm.x, palmNormTagNum); XML.setValue("PN:Y", palmNorm.y, palmNormTagNum); XML.setValue("PN:Z", palmNorm.z, palmNormTagNum); //--------------- // Export the hand basis matrix Leap::Matrix handMatrix = hand.basis(); ofPoint handBasisX = leap.getofPoint( handMatrix.xBasis); ofPoint handBasisY = leap.getofPoint( handMatrix.yBasis); ofPoint handBasisZ = leap.getofPoint( handMatrix.zBasis); int handMatrixTagNum = XML.addTag("HM"); if( XML.pushTag("HM", handMatrixTagNum) ){ XML.setValue("XX", handBasisX.x, handMatrixTagNum); XML.setValue("XY", handBasisX.y, handMatrixTagNum); XML.setValue("XZ", handBasisX.z, handMatrixTagNum); XML.setValue("YX", handBasisY.x, handMatrixTagNum); XML.setValue("YY", handBasisY.y, handMatrixTagNum); XML.setValue("YZ", handBasisY.z, handMatrixTagNum); XML.setValue("ZX", handBasisZ.x, handMatrixTagNum); XML.setValue("ZY", handBasisZ.y, handMatrixTagNum); XML.setValue("ZZ", handBasisZ.z, handMatrixTagNum); XML.popTag(); // pop HM (Hand Matrix) } //--------------- // Export the arm basis matrix Leap::Matrix armMatrix = arm.basis(); ofPoint armBasisX = leap.getofPoint( armMatrix.xBasis); ofPoint armBasisY = leap.getofPoint( armMatrix.yBasis); ofPoint armBasisZ = leap.getofPoint( armMatrix.zBasis); int armMatrixTagNum = XML.addTag("AM"); if( XML.pushTag("AM", armMatrixTagNum) ){ XML.setValue("XX", armBasisX.x, armMatrixTagNum); XML.setValue("XY", armBasisX.y, armMatrixTagNum); XML.setValue("XZ", armBasisX.z, armMatrixTagNum); XML.setValue("YX", armBasisY.x, armMatrixTagNum); XML.setValue("YY", armBasisY.y, armMatrixTagNum); XML.setValue("YZ", armBasisY.z, armMatrixTagNum); XML.setValue("ZX", armBasisZ.x, armMatrixTagNum); XML.setValue("ZY", armBasisZ.y, armMatrixTagNum); XML.setValue("ZZ", armBasisZ.z, armMatrixTagNum); XML.popTag(); // pop AM (Arm Matrix) } } } }
int main(int argc, const char **argv){ Cartesian c; string url(HOST); const string user(USER); const string pass(PASS); const string database(DB); string input_string, tmp, ui_input; vector<string> strs; string command_str; int speed, fWait; float distance; Logger logger; logger.setDebugLevel(Logger::INFO); PID GripPID(GripServo::MAX, GripServo::MIN); PID WrotPID(WrotServo::MAX, WrotServo::MIN); PID WristPID(WristServo::MAX, WristServo::MIN); PID ElbowPID(ElbowServo::MAX, ElbowServo::MIN); PID ShoulderPID(ShoulderServo::MAX, ShoulderServo::MIN); PID BasePID(BaseServo::MAX, BaseServo::MIN); Arm *currArm = new Arm(); Arm *prevArm = new Arm(); ForbiddenZone forbiddenZone; try{ //connect to database sql::Connection *con; sql::mysql::MySQL_Driver * driver; driver = sql::mysql::get_mysql_driver_instance(); con = driver->connect(url, user, pass); con->setSchema(database); logger.i("Database is connected"); std::auto_ptr< sql::Statement > stmt(con->createStatement()); std::auto_ptr< sql::ResultSet > res; while(1){ stmt->execute("CALL get_xyz(@rs)"); res.reset(stmt->executeQuery("select @rs as _val")); while (res->next()) { tmp = res->getString("_val"); strs = split(tmp, ' '); } if(tmp.length() == 0){ usleep(SLEEP_TIME); continue; } logger.i("Data available..."); if(strs.size() != XYZ_INPUT_COUNT){ logger.d("Ignore this bad input:" + emerge(strs, ' ')); continue; } logger.i("processing input:" + emerge(strs, ' ')); command_str = strs[0]; if(command_str == CONTROLLING_DISABLE) { logger.i("Controlling is not enabled!"); continue; } //set goal XYZ Position c.setX(atof(strs[1].c_str()) + 50); c.setY(atof(strs[2].c_str())); c.setZ(atof(strs[3].c_str()) - BASE_HEIGHT*2 - 50); if(c.getZ() < -50 || forbiddenZone.contains(c)){ logger.d("Ignore input..."); logger.d(c.toString()); continue; } currArm->setPosition(c); currArm->setWristAngle(atof(strs[4].c_str())); currArm->setGripLength(atof(strs[8].c_str())); currArm->setWrotAngle(atof(strs[5].c_str())); if(!currArm->convertXYZToRaw()){ logger.d("Input ignore..."); logger.d(currArm->toString()); continue; } ui_input = (currArm->getElbowXYZ()).toUICommandString(); ui_input += " "; ui_input += (currArm->getWristXYZ()).toUICommandString(); ui_input += " "; ui_input += c.toUICommandString(); ui_input += " "; ui_input += to_string(currArm->getWrotAngle()); ui_input += " "; ui_input += to_string(currArm->getGripLength()); logger.i(currArm->toString()); fWait = atoi(strs[7].c_str()); int change = getBiggestChange(currArm,prevArm); speed = change / PROPER_SPEED; prevArm->setToArm(currArm); input_string = "CALL add_input('"; input_string += command_str; input_string += Helper::formatString(to_string(currArm->getBaseRaw()),3); input_string += Helper::formatString(to_string(currArm->getShoulderRaw()),3); input_string += Helper::formatString(to_string(currArm->getElbowRaw()),3); input_string += Helper::formatString(to_string(currArm->getWristRaw()),3); input_string += Helper::formatString(to_string(currArm->getWrotRaw()),3); input_string += Helper::formatString(to_string(currArm->getGripRaw()),3); input_string += Helper::formatString(to_string(speed),4); input_string += Helper::formatString(to_string(fWait),1); input_string += "')"; logger.i("Success:" + input_string); stmt->execute(input_string); stmt->execute("CALL add_ui_input('"+ui_input+"')"); usleep(speed); } }catch(sql::SQLException &e){ logger.e(e.what()); return EXIT_FAILURE; } }