├── .gitignore ├── CHANGELOG.md ├── CMakeLists.txt ├── COPYING ├── COPYING.LESSER ├── README.md ├── VERSION ├── back ├── aibo.back ├── aibo_demo.back ├── allnight.back ├── bowling.back ├── calibrate.back ├── clearWorkspace.back ├── clearWorkspace.back.comments ├── comment.back ├── fileio.back ├── freeCurves.back ├── helloworld.back ├── init.back ├── kids.back ├── kinect2.back ├── kuka.back ├── movo.back ├── practice.back ├── scan.back ├── scan.back.comments ├── scene.back ├── sort.back ├── todo.back ├── unittest.back ├── utils.back ├── varianceTrial.back ├── washer_and_bolt.back └── wiki.back ├── baxter.sh ├── baxter.urdf ├── baxter_local.sh ├── bump-version.sh ├── cmake ├── FindOpenCL.cmake ├── FindPackageHandleStandardArgs.cmake └── FindPackageMessage.cmake ├── default ├── config │ ├── defaultCamera │ │ └── cameraCalibration.yml │ ├── leftIkMap │ ├── leftIkMapAtHeight │ ├── left_hand_camera │ │ ├── cameraCalibration.yml │ │ └── gripperMask.bmp │ ├── rightIkMap │ └── right_hand_camera │ │ ├── cameraCalibration.yml │ │ └── gripperMask.bmp ├── config_011310P0011 │ ├── leftIkMap │ ├── leftIkMapAtHeight │ ├── left_hand_camera │ │ ├── cameraCalibration.yml │ │ └── gripperMask.bmp │ ├── rightIkMap │ └── right_hand_camera │ │ ├── cameraCalibration.yml │ │ └── gripperMask.bmp ├── config_011509P0027 │ ├── config.yml │ ├── leftIkMap │ ├── leftIkMapAtHeight │ ├── left_hand_camera │ │ ├── cameraCalibration.yml │ │ └── gripperMask.bmp │ ├── left_kinect2_color_qhd │ │ ├── cameraCalibration.yml │ │ └── gripperMask.bmp │ ├── left_kinect2_depth │ │ └── gripperMask.bmp │ ├── left_kinect2_ir │ │ └── gripperMask.bmp │ ├── rightIkMap │ ├── rightIkMapAtHeight │ └── right_hand_camera │ │ ├── cameraCalibration.yml │ │ └── gripperMask.bmp ├── config_011509P0030 │ ├── config.yml │ ├── leftIkMap │ ├── leftIkMapAtHeight │ ├── left_hand_camera │ │ ├── cameraCalibration.yml │ │ └── gripperMask.bmp │ ├── left_kinect2_color_qhd │ │ ├── cameraCalibration.yml │ │ └── gripperMask.bmp │ ├── rightIkMap │ └── right_hand_camera │ │ ├── cameraCalibration.yml │ │ └── gripperMask.bmp ├── config_011601P0011 │ ├── config.yml │ ├── leftIkMap │ ├── leftIkMapAtHeight │ ├── left_hand_camera │ │ ├── cameraCalibration.yml │ │ └── gripperMask.bmp │ ├── rightIkMap │ └── right_hand_camera │ │ ├── cameraCalibration.yml │ │ └── gripperMask.bmp ├── maps │ ├── 011310P0011_bg_left.yml │ ├── 011310P0011_bg_right.yml │ ├── 011509P0030_bg_right.yml │ ├── 011601P0011_bg_left.yml │ ├── 011601P0011_bg_right.yml │ ├── simulatedserial_bg_left.yml │ └── simulatedserial_bg_right.yml ├── objects │ ├── baxterNarrowLongFinger │ │ └── ein │ │ │ ├── 3dGrasps │ │ │ └── 3dGrasps.yml │ │ │ ├── ir2d │ │ │ ├── ir2d.png │ │ │ └── ir2d.yml │ │ │ ├── pickMemories │ │ │ └── graspMemories.yml │ │ │ ├── sceneModel │ │ │ └── model.yml │ │ │ └── servoCrops │ │ │ ├── aerialHeight0Gradients.yml │ │ │ ├── aerialHeight1Gradients.yml │ │ │ ├── aerialHeight2Gradients.yml │ │ │ ├── aerialHeight3Gradients.yml │ │ │ ├── servoCrop0.png │ │ │ ├── servoCrop1.png │ │ │ ├── servoCrop2.png │ │ │ └── servoCrop3.png │ ├── knn.yml │ ├── labels.yml │ └── vocab.yml └── simulator │ ├── sprites │ ├── blueBowl │ │ └── image.ppm │ └── brush │ │ └── image.ppm │ └── tableTile.png ├── doc ├── README ├── _config.yml ├── _data │ └── ein_version.yml ├── _includes │ ├── ein_words.html │ ├── ein_words.txt │ ├── footer.html │ ├── head.html │ └── header.html ├── _layouts │ ├── default.html │ ├── page.html │ └── post.html ├── _posts │ └── 2016-06-10-welcome-to-ein.markdown ├── _sass │ ├── _base.scss │ ├── _layout.scss │ └── _syntax-highlighting.scss ├── about.md ├── aibo.md ├── architecture.md ├── assets │ ├── allenwrench.jpg │ ├── allenwrench_density.jpg │ ├── allenwrench_observed.jpg │ ├── baxter_crane.jpg │ ├── bg.jpg │ ├── bg_badgrippermask.jpg │ ├── bg_discrepancy.jpg │ ├── bg_observed.jpg │ ├── console_screenshot.jpg │ ├── einmainwindow_screenshot.jpg │ ├── grippermask1.jpg │ ├── grippermask2.jpg │ ├── grippermask3.jpg │ ├── magic_circle.jpg │ ├── narrow_finger_scene.jpg │ ├── renderedwristview.jpg │ ├── renderedwristview_backwardscalibrated.jpg │ ├── renderedwristview_calibrated1.jpg │ ├── renderedwristview_calibrated2.jpg │ ├── renderedwristview_calibrated3.jpg │ ├── renderedwristview_calibrated4.jpg │ ├── renderedwristview_nullcalibrated.jpg │ ├── wristview.jpg │ ├── wristviewLegend.odg │ └── wristviewLegend.png ├── calibration.md ├── cameras.md ├── css │ └── main.scss ├── data.md ├── faq.md ├── feed.xml ├── getstarted.md ├── index.html ├── install.md ├── language.md ├── lightfields.md ├── movement.md ├── movo.md ├── pickingobjects.md ├── pose.md ├── ros.md ├── scanningobjects.md ├── scene.md ├── simulation.md ├── stream.md └── words.md ├── ein_aibo.screenrc ├── ein_baxter.screenrc ├── ein_baxter.vimrc ├── ein_client.screenrc ├── ein_jaco.screenrc ├── ein_kuka.screenrc ├── ein_movo.screenrc ├── ein_pidrone.screenrc ├── how-to-release.txt ├── images ├── black.tif ├── calibration │ ├── magicpaper.odg │ ├── magicpaper.pdf │ └── magicpaper.png ├── gaze30006.tif ├── gaze30007.tif ├── gaze30008.tif ├── gaze30009.tif ├── gaze30010.tif ├── gaze30011.tif ├── gaze30012.tif ├── ursula_neutral.tif ├── ursula_no.tif └── ursula_yes.tif ├── launch ├── aibo.launch └── aibo.launch.bk ├── msg ├── EinConsole.msg └── EinState.msg ├── package.xml ├── python ├── bandit.py ├── draw_policy.py ├── ein_client.py ├── ein_print_console.py ├── ein_print_stack.py ├── ein_print_state.py ├── incomplete_beta_integral.py ├── readMocData.py ├── simple_regret_stopping_bandit.py ├── test.py ├── upload_zips.py └── warehouse_client.py ├── scripts └── sendToBothArms.sh ├── setup.sh ├── src ├── aibo │ ├── ein_aibo.cpp │ ├── ein_aibo.h │ ├── ein_aibo_config.h │ └── ein_aibo_reactive.cpp ├── base64.cpp ├── base64.h ├── baxter │ ├── ein_baxter.cpp │ ├── ein_baxter.h │ ├── ein_baxter_config.h │ ├── ein_face_animation.cpp │ ├── ein_ik.cpp │ ├── ein_ik.h │ └── ikfast │ │ ├── baxter_left_arm_ikfast_solver.cpp │ │ ├── baxter_right_arm_ikfast_solver.cpp │ │ ├── ikfast.h │ │ ├── ikfast_wrapper.cpp │ │ ├── ikfast_wrapper.h │ │ ├── ikfast_wrapper_left.cpp │ │ ├── ikfast_wrapper_left.h │ │ ├── ikfast_wrapper_right.cpp │ │ └── ikfast_wrapper_right.h ├── camera.cpp ├── camera.h ├── cephes │ ├── Makefile │ ├── airy.c │ ├── bdtr.c │ ├── beta.c │ ├── btdtr.c │ ├── cbrt.c │ ├── cephes_names.h │ ├── chbevl.c │ ├── chdtr.c │ ├── const.c │ ├── cpmul.c │ ├── dawsn.c │ ├── ellie.c │ ├── ellik.c │ ├── ellpe.c │ ├── ellpj.c │ ├── ellpk.c │ ├── exp10.c │ ├── exp2.c │ ├── expn.c │ ├── fdtr.c │ ├── fresnl.c │ ├── gamma.c │ ├── gdtr.c │ ├── gels.c │ ├── hyp2f1.c │ ├── hyperg.c │ ├── i0.c │ ├── i1.c │ ├── igam.c │ ├── igami.c │ ├── incbet.c │ ├── incbi.c │ ├── j0.c │ ├── j1.c │ ├── jv.c │ ├── k0.c │ ├── k1.c │ ├── kn.c │ ├── kolmogorov.c │ ├── mconf.h │ ├── mmmpy.c │ ├── mtherr.c │ ├── mtransp.c │ ├── mvmpy.c │ ├── nbdtr.c │ ├── ndtr.c │ ├── ndtri.c │ ├── pdtr.c │ ├── polevl.c │ ├── polmisc.c │ ├── polrt.c │ ├── polyn.c │ ├── powi.c │ ├── protos.h │ ├── psi.c │ ├── rgamma.c │ ├── round.c │ ├── scipy_iv.c │ ├── setprec.c │ ├── shichi.c │ ├── sici.c │ ├── simpsn.c │ ├── simq.c │ ├── sincos.c │ ├── sindg.c │ ├── spence.c │ ├── stdtr.c │ ├── struve.c │ ├── tandg.c │ ├── tukey.c │ ├── unity.c │ ├── yn.c │ ├── zeta.c │ └── zetac.c ├── compress.cpp ├── compress.h ├── config.cpp ├── config.h ├── distributions.c ├── distributions.h ├── eePose.cpp ├── eePose.h ├── eigen_util.cpp ├── eigen_util.h ├── ein.cpp ├── ein.h ├── ein_misc.cpp ├── ein_movement.cpp ├── ein_pick_place.cpp ├── ein_render.cpp ├── ein_scanning.cpp ├── ein_servo.cpp ├── ein_stream.cpp ├── ein_util.cpp ├── ein_util.h ├── ein_vision_cycle.cpp ├── ein_words.cpp ├── ein_words.h ├── faces.cpp ├── faces.h ├── gaussian_map.cpp ├── gaussian_map.h ├── gaussian_map_words.cpp ├── gaussian_map_words.h ├── jaco │ ├── ein_jaco.cpp │ ├── ein_jaco.h │ └── ein_jaco_config.h ├── kuka │ ├── ein_kuka.cpp │ ├── ein_kuka.h │ └── ein_kuka_config.h ├── movo │ ├── ein_movo.cpp │ ├── ein_movo.h │ └── ein_movo_config.h ├── opencl │ ├── ein_opencl.cpp │ └── oclNbody │ │ ├── Makefile │ │ ├── inc │ │ ├── cmd_arg_reader.h │ │ ├── oclBodySystem.h │ │ ├── oclBodySystemCpu.h │ │ ├── oclBodySystemOpencl.h │ │ ├── oclBodySystemOpenclLaunch.h │ │ ├── oclRenderParticles.h │ │ ├── oclUtils.h │ │ ├── param.h │ │ ├── paramgl.h │ │ ├── shrQATest.h │ │ └── shrUtils.h │ │ ├── obj │ │ └── release │ │ │ ├── cmd_arg_reader.cpp.o │ │ │ ├── oclBodySystemCpu.cpp.o │ │ │ ├── oclBodySystemOpencl.cpp.o │ │ │ ├── oclBodySystemOpenclLaunch.cpp.o │ │ │ ├── oclNbody.cpp.o │ │ │ ├── oclNbodyGold.cpp.o │ │ │ ├── oclRenderParticles.cpp.o │ │ │ ├── oclUtils.cpp.o │ │ │ ├── param.cpp.o │ │ │ ├── paramgl.cpp.o │ │ │ └── shrUtils.cpp.o │ │ ├── oclNbody.ptx │ │ ├── oclNbody.txt │ │ ├── oclNbodyKernel.cl │ │ └── src │ │ ├── cmd_arg_reader.cpp │ │ ├── oclBodySystemCpu.cpp │ │ ├── oclBodySystemOpencl.cpp │ │ ├── oclBodySystemOpenclLaunch.cpp │ │ ├── oclNbody.cpp │ │ ├── oclNbodyGold.cpp │ │ ├── oclNbodyMain.cpp │ │ ├── oclRenderParticles.cpp │ │ ├── oclUtils.cpp │ │ ├── param.cpp │ │ ├── paramgl.cpp │ │ └── shrUtils.cpp ├── pidrone │ ├── ein_pidrone.cpp │ ├── ein_pidrone.h │ └── ein_pidrone_config.h ├── qtgui │ ├── armwidget.cpp │ ├── armwidget.h │ ├── armwidget.ui │ ├── capturelineedit.cpp │ ├── capturelineedit.h │ ├── discrepancywindow.cpp │ ├── discrepancywindow.h │ ├── discrepancywindow.ui │ ├── einwindow.cpp │ ├── einwindow.h │ ├── einwindow.ui │ ├── gaussianmapwidget.cpp │ ├── gaussianmapwidget.h │ ├── gaussianmapwidget.ui │ ├── gaussianmapwindow.cpp │ ├── gaussianmapwindow.h │ ├── gaussianmapwindow.ui │ ├── main.cpp │ ├── mainwindow.cpp │ ├── mainwindow.h │ ├── mainwindow.ui │ ├── qt_util.cpp │ ├── qt_util.h │ ├── stackmodel.cpp │ ├── stackmodel.h │ ├── streamviewerwindow.cpp │ ├── streamviewerwindow.h │ ├── streamviewerwindow.ui │ ├── untitled.pro │ ├── untitled.pro.user │ ├── window_QT.cpp │ ├── window_QT.h │ ├── windowmanager.cpp │ ├── windowmanager.h │ ├── wordaction.cpp │ └── wordaction.h ├── randomkit.c ├── randomkit.h ├── repl.cpp ├── slu │ ├── gsl_utilities.cpp │ ├── gsl_utilities.h │ ├── math2d.cpp │ ├── math2d.h │ ├── math3d.cpp │ └── math3d.h ├── tokenizer.hpp ├── word.cpp └── word.h └── timecpp.sh /.gitignore: -------------------------------------------------------------------------------- 1 | *#* 2 | .#* 3 | *.pyc 4 | *.aux 5 | *.bbl 6 | *.blg 7 | *.log 8 | *~ 9 | classCrops/ 10 | savedCrops/ 11 | data2/ 12 | data3/ 13 | data4/ 14 | data5/ 15 | data6/ 16 | data30weak/ 17 | 18 | .*.swp 19 | *.sw* 20 | CMakeCache.txt 21 | CMakeFiles/ 22 | .~lock* 23 | *-crop.pdf 24 | *-pics.pdf 25 | model.pdf 26 | 27 | *.out 28 | oomdp/bin 29 | 30 | default/objects/*/raw/* 31 | default/objects/*/ein/detectionCrops/ 32 | *.sv? 33 | *.sv? 34 | 35 | back/practice.back 36 | default/objects 37 | .sass-cache 38 | _site 39 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Ein 2 | 3 | Ein is pick-and-place software for Baxter. See the installation 4 | instructions on our web site: http://h2r.github.io/ein/. 5 | 6 | Ein can be used to control Baxter, make models of objects, detect, 7 | localize, and pick objects using these models. We can localize 8 | objects with Baxter to within 2mm, and pick objects hundreds of times 9 | in a row without a failure. The robot can also be used to create a 10 | "for" loop over objects, picking from an input pile, playing with an 11 | object (doing an arbitrary learning behavior that you choose), and 12 | then moving to the output pile. We have automatically processed 13 | hundreds of objects in this way as part of the Million Object 14 | Challenge. 15 | 16 | 17 | We follow this branching model for Ein releases: 18 | http://nvie.com/posts/a-successful-git-branching-model/ so the 19 | development branch is develop, and master always contains the latest 20 | release. -------------------------------------------------------------------------------- /VERSION: -------------------------------------------------------------------------------- 1 | 1.1 2 | -------------------------------------------------------------------------------- /back/aibo_demo.back: -------------------------------------------------------------------------------- 1 | demoDogBark 2 | demoDogSit 3 | demoDogLieDown 4 | 5 | -------------------------------------------------------------------------------- /back/allnight.back: -------------------------------------------------------------------------------- 1 | 2 | 3 | ( 4 | blankFace 5 | lightsOff 6 | untuckArms 7 | disableRobot 8 | ) 9 | "robotOff" store 10 | 11 | ( 12 | happyFace 13 | lightsOn 14 | untuckArms 15 | ) 16 | "robotOn" store 17 | 18 | 19 | 20 | ( 21 | sadFace 22 | lightsOff 23 | untuckArms 24 | disableRobot 25 | ) 26 | "robotDroop" store 27 | 28 | 29 | ( 30 | fullImpulse assumeBeeHome waitUntilAtCurrentPosition 31 | zDown zDown zDown zDown zDown zDown zDown zDown waitUntilAtCurrentPosition closeGripper zUp zUp zUp zUp sadFace lightsOff 32 | ) "attemptPick" store 33 | 34 | ( 35 | 0.01 setGridSize 36 | 0.04 setW1GoThresh 0.1 setW1AngleThresh 37 | 0.2 setSpeed 38 | openGripper 39 | ) "pressPick_init" store 40 | 41 | 42 | ( 43 | openGripper touchDown zUp zUp zUp waitUntilAtCurrentPosition closeGripper waitUntilGripperNotMoving 44 | 45 | 0 changeToHeight zDown zDown zDown 46 | /* assumeBeeHome */ 47 | ) "pressPick" store 48 | 49 | ( 50 | 0.01 setGridSize 51 | 0.1 setW1GoThresh 0.2 setW1AngleThresh 52 | ( zUp ) 30 replicateWord waitUntilAtCurrentPosition 53 | ( zDown ) 30 replicateWord waitUntilAtCurrentPosition 54 | ) "shake" store 55 | 56 | ( 57 | pressPick_init pressPick 0.5 setSpeed ( shake ) 3 replicateWord openGripper 58 | ) "grabShake" store 59 | 60 | 61 | ( 62 | 0.05 setW1GoThresh 0.05 setW1AngleThresh 63 | 0.04 setGridSize 64 | 0.2 setSpeed 65 | leftOrRightArm "right" = ( assumeBeeHome xDown xDown xDown xDown xDown xDown xDown xDown ) ift 66 | leftOrRightArm "left" = ( assumeBeeHome xDown yDown xDown yDown xDown yDown ) ift 67 | ) "fakeIrScan_init" store 68 | 69 | 70 | ( 71 | ( ( xUp ) 10 replicateWord waitUntilAtCurrentPosition 72 | yUp waitUntilAtCurrentPosition 73 | ( xDown ) 10 replicateWord waitUntilAtCurrentPosition 74 | yUp waitUntilAtCurrentPosition ) 3 replicateWord 75 | ) "fakeIrScan" store 76 | 77 | 78 | ( 79 | happyFace fullImpulse openGripper 80 | assumeCrane1 waitUntilCurrentPosition 81 | ) 82 | "intro_init" store 83 | 84 | 85 | 86 | 87 | ( 88 | .20 waitForSeconds 89 | 40 openGripperInt 90 | .20 waitForSeconds 91 | 0 openGripperInt 92 | ) "squeakDuck" store 93 | 94 | 95 | -------------------------------------------------------------------------------- /back/bowling.back: -------------------------------------------------------------------------------- 1 | 2 | "bowlingPin" deliverObject 3 | ( oZUp ) 10 replicateWord 4 | touchDown -------------------------------------------------------------------------------- /back/comment.back: -------------------------------------------------------------------------------- 1 | /* testing if nested comments are supported (right now they are not and give an error) */ 2 | 3 | /* this is a comment */ 4 | 5 | /* this is a nested comment 6 | 7 | /* nested */ 8 | 9 | */ 10 | 11 | 12 | -------------------------------------------------------------------------------- /back/fileio.back: -------------------------------------------------------------------------------- 1 | "test" fileOpenOutput "outputFile" store 2 | outputFile "test 1234" fileWrite 3 | outputFile fileClose 4 | 5 | "test" fileOpenInput "inputFile" store 6 | inputFile fileReadAll 7 | 8 | "test 1234" = assert 9 | inputFile fileClose 10 | 11 | "test" fileOpenOutput "outputFile" store 12 | outputFile currentPose fileWriteLine 13 | outputFile currentPose fileWriteLine 14 | outputFile currentPose fileWriteLine 15 | outputFile fileClose 16 | 17 | 18 | "test" fileOpenInput fileReadAll eval /* evalaute the file and put it on the stack. */ 19 | currentPose = assert 20 | currentPose = assert 21 | currentPose = assert 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /back/freeCurves.back: -------------------------------------------------------------------------------- 1 | 2 | 0.1 "freeSpeed" store 3 | 4 | ( 5 | setControlModeAngles 6 | brandish 7 | 8 | ) "equipFreeCurves" store 9 | 10 | 11 | ( 12 | -0.112277 13 | -0.483173 14 | 0.0530297 15 | 0.511892 16 | -0.117036 17 | 0.672303 18 | -0.113986 19 | moveJointsToAngles 20 | ) "brandish" store 21 | 22 | /* takes a double on the stack */ 23 | ( 24 | 1 dsr 25 | "sliceSpeed" store 26 | 0 27 | 0.1 sliceSpeed times 28 | 0 29 | 0.15 sliceSpeed times 30 | 0 31 | 0.15 sliceSpeed times 32 | 0 33 | moveJointsByAngles 34 | ) "slice" store 35 | 36 | 37 | /* takes a double on the stack */ 38 | ( 39 | 1 dsr 40 | "slapSpeed" store 41 | 0.1 slapSpeed times 42 | 0 43 | 0 44 | 0 45 | 0 46 | 0 47 | 0 48 | moveJointsByAngles 49 | ) "slap" store 50 | 51 | /* takes a double on the stack */ 52 | ( 53 | 1 dsr 54 | "swingSpeed" store 55 | 0.1 swingSpeed times 56 | 0 57 | 0.1 swingSpeed times 58 | 0 59 | 0.1 swingSpeed times 60 | 0 61 | 0 62 | moveJointsByAngles 63 | ) "swing" store 64 | 65 | 66 | ( 67 | /* 68 | 0 69 | -0.483173 1 pushCurrentJointAngle minus 70 | 0 71 | 0.511892 3 pushCurrentJointAngle minus 72 | 0 73 | 0.672303 5 pushCurrentJointAngle minus 74 | 0 75 | moveJointsByAngles 76 | */ 77 | 78 | 0 79 | 0 80 | 0 81 | -0.0490874 3 pushCurrentJointAngle minus 82 | 0 83 | 0.11083 5 pushCurrentJointAngle minus 84 | 0 85 | moveJointsByAngles 86 | ) "stab" store 87 | 88 | ( 89 | /* 90 | -0.112277 0 pushCurrentJointAngle minus 91 | 0 92 | 0.0530297 2 pushCurrentJointAngle minus 93 | 0 94 | -0.117036 4 pushCurrentJointAngle minus 95 | 0 96 | -0.113986 6 pushCurrentJointAngle minus 97 | moveJointsByAngles 98 | */ 99 | 100 | 0 101 | 0 102 | 0.0530297 2 pushCurrentJointAngle minus 103 | 0 104 | -0.117036 4 pushCurrentJointAngle minus 105 | 0 106 | -0.113986 6 pushCurrentJointAngle minus 107 | moveJointsByAngles 108 | ) "swat" store 109 | 110 | ( 111 | swat stab -2.0 slice stab 112 | ) "raise" store 113 | 114 | ( 115 | 0 116 | -0.483173 1 pushCurrentJointAngle minus 117 | 0 118 | 0 119 | 0 120 | 0 121 | 0 122 | moveJointsByAngles 123 | ) "levelOut" store 124 | 125 | /* takes two doubles on the stack */ 126 | ( 127 | 2 dsr 128 | "cutSharp" store 129 | "cutLength" store 130 | 131 | .6 cutLength times 132 | slice 133 | 134 | 1.0 cutLength times 135 | cutSharp times 136 | swing 137 | 138 | .2 cutLength times 139 | cutSharp 0 < ( 140 | -1.0 141 | ) ( 142 | 1.0 143 | ) ifte 144 | times 145 | slap 146 | 147 | ) "cut" store 148 | 149 | 150 | 151 | 152 | 153 | 154 | -------------------------------------------------------------------------------- /back/helloworld.back: -------------------------------------------------------------------------------- 1 | ( "hello world" dup print ) "hello" store 2 | 3 | hello 4 | 5 | -------------------------------------------------------------------------------- /back/init.back: -------------------------------------------------------------------------------- 1 | "scene" import 2 | "scan" import 3 | "calibrate" import 4 | 5 | "utils" import 6 | "clearWorkspace" import 7 | 8 | "wiki" import 9 | 10 | robotType "baxter" = 11 | ( 12 | 13 | happyFace 14 | 15 | tableInit 16 | 17 | ( 18 | assumeBeeHome 19 | 20 | /* 21 | leftOrRightArm "right" = 22 | ( 23 | 0.607890 -0.388495 0.098883 0.000585 0.999998 0.000919 0.001453 createEEPose moveEeToPoseWord 24 | ) 25 | ( 26 | leftOrRightArm "left" = 27 | ( 28 | assumeBeeHome 29 | ) 30 | ( "Neither left or right arm." leftOrRighArm pauseStackExecution ) 31 | ifte 32 | ) 33 | ifte 34 | */ 35 | 36 | 37 | 1 changeToHeight waitUntilAtCurrentPosition 38 | 39 | shoreUp 40 | ) "Go to the home position, wait until at current position, and put the joints in a good state." "goHome" define 41 | 42 | cameraWristConfig 43 | 44 | ) 45 | ift 46 | 47 | 48 | endArgs "baxterNarrowLongFinger" setClassLabels 49 | -------------------------------------------------------------------------------- /back/kinect2.back: -------------------------------------------------------------------------------- 1 | leftOrRightArm "_kinect2_color_qhd" + "/kinect2/qhd/image_color" leftOrRightArm "_hand" + "k2rgb_tf_link" cameraCreate 2 | 3 | leftOrRightArm "_kinect2_ir" + "/kinect2/sd/image_ir" leftOrRightArm "_hand" + "k2ir_tf_link" cameraCreate 4 | 5 | leftOrRightArm "_kinect2_depth" + "/kinect2/sd/image_depth" leftOrRightArm "_hand" + "k2ir_tf_link" cameraCreate 6 | -------------------------------------------------------------------------------- /back/kuka.back: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/back/kuka.back -------------------------------------------------------------------------------- /back/sort.back: -------------------------------------------------------------------------------- 1 | 2 | ( 1 2 3 4 ) car 1 = 3 | assert 4 | 5 | ( 1 2 3 4 ) cdr ( 2 3 4 ) = 6 | assert 7 | 8 | ( 1 2 3 4 ) ( 1 2 3 4 ) = 9 | assert 10 | 11 | ( 1 2 3 4 ) ( 1 2 3 4 5 ) = 12 | assertNo 13 | 14 | ( 1 2 3 4 ) ( 2 2 3 4 ) = 15 | assertNo 16 | 17 | 18 | 19 | 1 2 cmp -1 = 20 | assert 21 | 22 | 2 1 cmp 1 = 23 | assert 24 | 25 | 2 2 cmp 0 = 26 | assert 27 | 28 | 29 | 1.1 2.3 cmp -1 = 30 | assert 31 | 32 | 2.3 1 cmp 1 = 33 | assert 34 | 35 | 2 2 cmp 0 = 36 | assert 37 | 38 | 2 2.0 cmp 0 = 39 | assert 40 | 41 | 2 2.000001 cmp -1 = 42 | assert 43 | 44 | "a" "b" cmp -1 = 45 | assert 46 | 47 | "b" "a" cmp 1 = 48 | assert 49 | 50 | "a" "a" cmp 0 = 51 | assert 52 | 53 | 54 | "a" "b" > 55 | assertNo 56 | 57 | "a" "b" >= 58 | assertNo 59 | 60 | "a" "b" < 61 | assert 62 | 63 | "a" "b" <= 64 | assert 65 | 66 | 67 | 68 | 69 | 1 2 < 1 = 70 | assert 71 | 72 | 2 1 < 1 = 73 | assertNo 74 | 75 | 76 | 1 2 > 1 = 77 | assertNo 78 | 79 | 2 1 > 1 = 80 | assert 81 | 82 | 1.0 2 > 1 = 83 | assertNo 84 | 85 | 2 1.0 > 1 = 86 | assert 87 | 88 | 89 | 90 | 91 | [ 0 ( dup inc ) 5 replicateWord ] ( dup * ) map 92 | ( 0 1 4 9 16 25 ) = 93 | assert 94 | 95 | 96 | 97 | ( 1 2 3 4 ) 2 get 3 = 98 | assert 99 | 100 | 101 | ( 1 2 3 4 ) 0 get 1 = 102 | assert 103 | 104 | ( 1 2 3 4 ) size 4 = 105 | assert 106 | 107 | -------------------------------------------------------------------------------- /back/unittest.back: -------------------------------------------------------------------------------- 1 | 3.342000 7.540000 3.620000 2.370000 -1.250000 0.990000 3.630000 createEEPose 2 | 3.342000 7.540000 3.620000 2.370000 -1.250000 0.990000 3.620000 createEEPose 3 | = 4 | assertNo 5 | 6 | 3.342000 7.540000 3.620000 2.370000 -1.250000 0.990000 3.620000 createEEPose 7 | 3.342000 7.540000 3.620000 2.370000 -1.250000 0.990000 3.620000 createEEPose 8 | = 9 | assert 10 | 11 | 12 | "right" "right1" = ! 13 | assert 14 | 15 | "right" "right" = 16 | assert 17 | 18 | "right" "right1" = 19 | assertNo 20 | 21 | "right" "right" = ! 22 | assertNo 23 | 24 | 25 | "right" "right" = "right" "left" ifte "right" = 26 | assert 27 | 28 | "rightx" "right" = "right" "left" ifte "left" = 29 | assert 30 | 31 | 32 | inf inf = 33 | assert 34 | 35 | 1 "infinite" store 36 | 1 infinite = assert 37 | 38 | 1 not assertNo 39 | 0 not assert 40 | 41 | 1 "x" store 42 | ( x 10 < ) ( x 1 + "x" store ) whileCollapsed 43 | x 10.0 = assert 44 | 45 | 1 "x" store 46 | ( x 10 < ) ( x 1 + "x" store ) while 47 | x 10.0 = assert 48 | 49 | "*" "x" store 50 | x "*" = assert 51 | 52 | 2 3 * 1 + 7 = assert 53 | 54 | 55 | 10 10.0 = assert 56 | 57 | 58 | /* This one should do 'x' because the truth value of a compound word is true */ 59 | ( 0 ) ( "x" ) ift "x" = assert 60 | 61 | /* This one should do 'x' because ift doesn't take a compound word */ 62 | 1 63 | 0 ( "x" ) ift 64 | 1 = assert 65 | 66 | "/" 1 1 + 2 = assert 67 | "/" = assert 68 | 1 2 - -1 = assert 69 | 2 1 - 1 = assert 70 | 71 | 11 2 % 1 = assert 72 | 10 2 % 0 = assert 73 | -------------------------------------------------------------------------------- /cmake/FindPackageMessage.cmake: -------------------------------------------------------------------------------- 1 | #.rst: 2 | # FindPackageMessage 3 | # ------------------ 4 | # 5 | # 6 | # 7 | # FIND_PACKAGE_MESSAGE( "message for user" "find result details") 8 | # 9 | # This macro is intended to be used in FindXXX.cmake modules files. It 10 | # will print a message once for each unique find result. This is useful 11 | # for telling the user where a package was found. The first argument 12 | # specifies the name (XXX) of the package. The second argument 13 | # specifies the message to display. The third argument lists details 14 | # about the find result so that if they change the message will be 15 | # displayed again. The macro also obeys the QUIET argument to the 16 | # find_package command. 17 | # 18 | # Example: 19 | # 20 | # :: 21 | # 22 | # if(X11_FOUND) 23 | # FIND_PACKAGE_MESSAGE(X11 "Found X11: ${X11_X11_LIB}" 24 | # "[${X11_X11_LIB}][${X11_INCLUDE_DIR}]") 25 | # else() 26 | # ... 27 | # endif() 28 | 29 | #============================================================================= 30 | # Copyright 2008-2009 Kitware, Inc. 31 | # 32 | # Distributed under the OSI-approved BSD License (the "License"); 33 | # see accompanying file Copyright.txt for details. 34 | # 35 | # This software is distributed WITHOUT ANY WARRANTY; without even the 36 | # implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 37 | # See the License for more information. 38 | #============================================================================= 39 | # (To distribute this file outside of CMake, substitute the full 40 | # License text for the above reference.) 41 | 42 | function(FIND_PACKAGE_MESSAGE pkg msg details) 43 | # Avoid printing a message repeatedly for the same find result. 44 | if(NOT ${pkg}_FIND_QUIETLY) 45 | string(REPLACE "\n" "" details "${details}") 46 | set(DETAILS_VAR FIND_PACKAGE_MESSAGE_DETAILS_${pkg}) 47 | if(NOT "${details}" STREQUAL "${${DETAILS_VAR}}") 48 | # The message has not yet been printed. 49 | message(STATUS "${msg}") 50 | 51 | # Save the find details in the cache to avoid printing the same 52 | # message again. 53 | set("${DETAILS_VAR}" "${details}" 54 | CACHE INTERNAL "Details about finding ${pkg}") 55 | endif() 56 | endif() 57 | endfunction() 58 | -------------------------------------------------------------------------------- /default/config/defaultCamera/cameraCalibration.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | savedTime: 3 | - 1.4635826635290873e+09 4 | hand_camera_offset: { px:3.8150000000000003e-02, 5 | py:1.1440000000000001e-02, pz:1.5890000000000001e-02, qw:1., qx:0., 6 | qy:0., qz:0. } 7 | currentTableZ: 8 | - 1.5148318273476732e-01 9 | cropUpperLeftCorner: 10 | - 320. 11 | - 200. 12 | vanishingPointReticle: 13 | - 309. 14 | - 163. 15 | heightReticles: 16 | - 324. 17 | - 326. 18 | - 330. 19 | - 338. 20 | - 140. 21 | - 133. 22 | - 124. 23 | - 105. 24 | colorReticles: 25 | - 462 26 | - 450 27 | - 439 28 | - 428 29 | - 419 30 | - 410 31 | - 405 32 | - 399 33 | - 394 34 | - 389 35 | - 383 36 | - 381 37 | - 379 38 | - 378 39 | - 153 40 | - 153 41 | - 153 42 | - 153 43 | - 153 44 | - 154 45 | - 154 46 | - 154 47 | - 154 48 | - 154 49 | - 155 50 | - 155 51 | - 155 52 | - 155 53 | lensCorrections: 54 | - 9.7500000000000031e-01 55 | - 9.3500000000000028e-01 56 | - 9.3500000000000028e-01 57 | - 9.3500000000000028e-01 58 | - 7.7500000000000013e-01 59 | - 7.3500000000000010e-01 60 | - 7.3500000000000010e-01 61 | - 7.3500000000000010e-01 62 | gear0offset: 63 | - 3.2000001519918442e-02 64 | - 2.6000000536441803e-02 65 | - 1.6722800210118294e-02 66 | - 0. 67 | cameraExposure: 80 68 | cameraGain: 5 69 | cameraWhiteBalanceRed: 1224 70 | cameraWhiteBalanceGreen: 1124 71 | cameraWhiteBalanceBlue: 1648 72 | -------------------------------------------------------------------------------- /default/config/left_hand_camera/cameraCalibration.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | savedTime: 3 | - 1.4635826635290873e+09 4 | currentTableZ: 5 | - 1.5148318273476732e-01 6 | cropUpperLeftCorner: 7 | - 320. 8 | - 200. 9 | vanishingPointReticle: 10 | - 309. 11 | - 163. 12 | heightReticles: 13 | - 324. 14 | - 326. 15 | - 330. 16 | - 338. 17 | - 140. 18 | - 133. 19 | - 124. 20 | - 105. 21 | colorReticles: 22 | - 462 23 | - 450 24 | - 439 25 | - 428 26 | - 419 27 | - 410 28 | - 405 29 | - 399 30 | - 394 31 | - 389 32 | - 383 33 | - 381 34 | - 379 35 | - 378 36 | - 153 37 | - 153 38 | - 153 39 | - 153 40 | - 153 41 | - 154 42 | - 154 43 | - 154 44 | - 154 45 | - 154 46 | - 155 47 | - 155 48 | - 155 49 | - 155 50 | lensCorrections: 51 | - 9.7500000000000031e-01 52 | - 9.3500000000000028e-01 53 | - 9.3500000000000028e-01 54 | - 9.3500000000000028e-01 55 | - 7.7500000000000013e-01 56 | - 7.3500000000000010e-01 57 | - 7.3500000000000010e-01 58 | - 7.3500000000000010e-01 59 | gear0offset: 60 | - 3.2000001519918442e-02 61 | - 2.6000000536441803e-02 62 | - 1.6722800210118294e-02 63 | - 0. 64 | cameraExposure: 80 65 | cameraGain: 5 66 | cameraWhiteBalanceRed: 1224 67 | cameraWhiteBalanceGreen: 1124 68 | cameraWhiteBalanceBlue: 1648 69 | -------------------------------------------------------------------------------- /default/config/left_hand_camera/gripperMask.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/default/config/left_hand_camera/gripperMask.bmp -------------------------------------------------------------------------------- /default/config/right_hand_camera/cameraCalibration.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | savedTime: 3 | - 1.4635824517116482e+09 4 | currentTableZ: 5 | - 1.3771361573157681e-01 6 | cropUpperLeftCorner: 7 | - 320. 8 | - 200. 9 | vanishingPointReticle: 10 | - 309. 11 | - 189. 12 | heightReticles: 13 | - 317. 14 | - 318. 15 | - 324. 16 | - 333. 17 | - 171. 18 | - 168. 19 | - 156. 20 | - 137. 21 | colorReticles: 22 | - 462 23 | - 450 24 | - 439 25 | - 428 26 | - 419 27 | - 410 28 | - 405 29 | - 399 30 | - 394 31 | - 389 32 | - 383 33 | - 381 34 | - 379 35 | - 378 36 | - 153 37 | - 153 38 | - 153 39 | - 153 40 | - 153 41 | - 154 42 | - 154 43 | - 154 44 | - 154 45 | - 154 46 | - 155 47 | - 155 48 | - 155 49 | - 155 50 | lensCorrections: 51 | - 1.2400000000000004e+00 52 | - 1.2400000000000004e+00 53 | - 1.2400000000000004e+00 54 | - 1.2400000000000004e+00 55 | - 1.7200000000000009e+00 56 | - 1.6600000000000008e+00 57 | - 1.6600000000000008e+00 58 | - 1.6600000000000008e+00 59 | gear0offset: 60 | - 2.3000000044703484e-02 61 | - 2.3000000044703484e-02 62 | - 1.6722800210118294e-02 63 | - 0. 64 | cameraExposure: 80 65 | cameraGain: 15 66 | cameraWhiteBalanceRed: 1124 67 | cameraWhiteBalanceGreen: 1024 68 | cameraWhiteBalanceBlue: 2048 69 | -------------------------------------------------------------------------------- /default/config/right_hand_camera/gripperMask.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/default/config/right_hand_camera/gripperMask.bmp -------------------------------------------------------------------------------- /default/config_011310P0011/left_hand_camera/cameraCalibration.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | savedTime: 3 | - 1.5235566095451727e+09 4 | hand_camera_offset: { px:7.4999999999999997e-02, 5 | py:8.9999999999999993e-03, pz:1.5890000000000001e-02, qw:1., qx:0., 6 | qy:0., qz:0. } 7 | cropUpperLeftCorner: 8 | - 320. 9 | - 200. 10 | mu: 11 | - 2.3999999999999998e-03 12 | - 2.4499999999999999e-03 13 | kappa: 14 | - 0. 15 | - 0. 16 | vanishingPointReticle: 17 | - 309. 18 | - 163. 19 | heightReticles: 20 | - 324. 21 | - 327. 22 | - 329. 23 | - 335. 24 | - 139. 25 | - 131. 26 | - 124. 27 | - 100. 28 | colorReticles: 29 | - 462 30 | - 450 31 | - 439 32 | - 428 33 | - 419 34 | - 410 35 | - 405 36 | - 399 37 | - 394 38 | - 389 39 | - 383 40 | - 381 41 | - 379 42 | - 378 43 | - 153 44 | - 153 45 | - 153 46 | - 153 47 | - 153 48 | - 154 49 | - 154 50 | - 154 51 | - 154 52 | - 154 53 | - 155 54 | - 155 55 | - 155 56 | - 155 57 | lensCorrections: 58 | - 8.1500000000000017e-01 59 | - 8.1500000000000017e-01 60 | - 8.1500000000000017e-01 61 | - 8.1500000000000017e-01 62 | - 1.0550000000000004e+00 63 | - 1.0150000000000003e+00 64 | - 1.0150000000000003e+00 65 | - 1.0150000000000003e+00 66 | gear0offset: 67 | - 3.2000001519918442e-02 68 | - 2.6000000536441803e-02 69 | - 1.6722800210118294e-02 70 | - 0. 71 | cameraExposure: 100 72 | cameraGain: 5 73 | cameraWhiteBalanceRed: 1024 74 | cameraWhiteBalanceGreen: 1024 75 | cameraWhiteBalanceBlue: 2272 76 | -------------------------------------------------------------------------------- /default/config_011310P0011/left_hand_camera/gripperMask.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/default/config_011310P0011/left_hand_camera/gripperMask.bmp -------------------------------------------------------------------------------- /default/config_011310P0011/right_hand_camera/cameraCalibration.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | savedTime: 3 | - 1.5235518482883341e+09 4 | hand_camera_offset: { px:3.8150000000000003e-02, 5 | py:1.1440000000000001e-02, pz:1.5890000000000001e-02, qw:1., qx:0., 6 | qy:0., qz:0. } 7 | cropUpperLeftCorner: 8 | - 320. 9 | - 200. 10 | mu: 11 | - 2.3999999999999998e-03 12 | - 2.4499999999999999e-03 13 | kappa: 14 | - 0. 15 | - 0. 16 | vanishingPointReticle: 17 | - 309. 18 | - 189. 19 | heightReticles: 20 | - 319. 21 | - 319. 22 | - 322. 23 | - 331. 24 | - 171. 25 | - 166. 26 | - 155. 27 | - 135. 28 | colorReticles: 29 | - 462 30 | - 450 31 | - 439 32 | - 428 33 | - 419 34 | - 410 35 | - 405 36 | - 399 37 | - 394 38 | - 389 39 | - 383 40 | - 381 41 | - 379 42 | - 378 43 | - 153 44 | - 153 45 | - 153 46 | - 153 47 | - 153 48 | - 154 49 | - 154 50 | - 154 51 | - 154 52 | - 154 53 | - 155 54 | - 155 55 | - 155 56 | - 155 57 | lensCorrections: 58 | - 1.2550000000000006e+00 59 | - 1.2550000000000006e+00 60 | - 1.2550000000000006e+00 61 | - 1.2550000000000006e+00 62 | - 1.0550000000000004e+00 63 | - 1.0550000000000004e+00 64 | - 1.0550000000000004e+00 65 | - 1.0550000000000004e+00 66 | gear0offset: 67 | - 2.3000000044703484e-02 68 | - 2.3000000044703484e-02 69 | - 1.6722800210118294e-02 70 | - 0. 71 | cameraExposure: 80 72 | cameraGain: 15 73 | cameraWhiteBalanceRed: 1124 74 | cameraWhiteBalanceGreen: 1024 75 | cameraWhiteBalanceBlue: 2048 76 | -------------------------------------------------------------------------------- /default/config_011310P0011/right_hand_camera/gripperMask.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/default/config_011310P0011/right_hand_camera/gripperMask.bmp -------------------------------------------------------------------------------- /default/config_011509P0027/config.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | savedTime: 3 | - 1.4855545515736668e+09 4 | currentTableZ: 5 | - 2.0353401462955936e-01 6 | -------------------------------------------------------------------------------- /default/config_011509P0027/left_hand_camera/cameraCalibration.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | savedTime: 3 | - 1.4812161758303955e+09 4 | hand_camera_offset: { px:2.7000000000000000e-02, 5 | py:1.2000000000000000e-02, pz:1.5890000000000001e-02, qw:1., qx:0., 6 | qy:0., qz:0. } 7 | cropUpperLeftCorner: 8 | - 320. 9 | - 200. 10 | mu: 11 | - 2.3999999999999998e-03 12 | - 2.4499999999999999e-03 13 | kappa: 14 | - 0. 15 | - 0. 16 | vanishingPointReticle: 17 | - 314. 18 | - 218. 19 | heightReticles: 20 | - 331. 21 | - 335. 22 | - 341. 23 | - 347. 24 | - 190. 25 | - 183. 26 | - 174. 27 | - 148. 28 | colorReticles: 29 | - 462 30 | - 450 31 | - 439 32 | - 428 33 | - 419 34 | - 410 35 | - 405 36 | - 399 37 | - 394 38 | - 389 39 | - 383 40 | - 381 41 | - 379 42 | - 378 43 | - 153 44 | - 153 45 | - 153 46 | - 153 47 | - 153 48 | - 154 49 | - 154 50 | - 154 51 | - 154 52 | - 154 53 | - 155 54 | - 155 55 | - 155 56 | - 155 57 | lensCorrections: 58 | - 2.9499999999999998e-01 59 | - 2.9499999999999998e-01 60 | - 2.9499999999999998e-01 61 | - 2.9499999999999998e-01 62 | - 1.0550000000000004e+00 63 | - 1.0150000000000003e+00 64 | - 1.0150000000000003e+00 65 | - 1.0150000000000003e+00 66 | gear0offset: 67 | - 3.2000001519918442e-02 68 | - 2.6000000536441803e-02 69 | - 1.6722800210118294e-02 70 | - 0. 71 | cameraExposure: 80 72 | cameraGain: 5 73 | cameraWhiteBalanceRed: 1124 74 | cameraWhiteBalanceGreen: 1024 75 | cameraWhiteBalanceBlue: 2048 76 | -------------------------------------------------------------------------------- /default/config_011509P0027/left_hand_camera/gripperMask.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/default/config_011509P0027/left_hand_camera/gripperMask.bmp -------------------------------------------------------------------------------- /default/config_011509P0027/left_kinect2_color_qhd/cameraCalibration.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | savedTime: 3 | - 1.4715546714731548e+09 4 | hand_camera_offset: { px:3.8150000000000003e-02, 5 | py:1.1440000000000001e-02, pz:1.5890000000000001e-02, qw:1., qx:0., 6 | qy:0., qz:0. } 7 | cropUpperLeftCorner: 8 | - 320. 9 | - 200. 10 | vanishingPointReticle: 11 | - 309. 12 | - 163. 13 | heightReticles: 14 | - 324. 15 | - 326. 16 | - 330. 17 | - 338. 18 | - 140. 19 | - 133. 20 | - 124. 21 | - 105. 22 | colorReticles: 23 | - 462 24 | - 450 25 | - 439 26 | - 428 27 | - 419 28 | - 410 29 | - 405 30 | - 399 31 | - 394 32 | - 389 33 | - 383 34 | - 381 35 | - 379 36 | - 378 37 | - 153 38 | - 153 39 | - 153 40 | - 153 41 | - 153 42 | - 154 43 | - 154 44 | - 154 45 | - 154 46 | - 154 47 | - 155 48 | - 155 49 | - 155 50 | - 155 51 | lensCorrections: 52 | - 9.7500000000000031e-01 53 | - 9.3500000000000028e-01 54 | - 9.3500000000000028e-01 55 | - 9.3500000000000028e-01 56 | - 7.7500000000000013e-01 57 | - 7.3500000000000010e-01 58 | - 7.3500000000000010e-01 59 | - 7.3500000000000010e-01 60 | gear0offset: 61 | - 3.2000001519918442e-02 62 | - 2.6000000536441803e-02 63 | - 1.6722800210118294e-02 64 | - 0. 65 | cameraExposure: 80 66 | cameraGain: 5 67 | cameraWhiteBalanceRed: 1224 68 | cameraWhiteBalanceGreen: 1124 69 | cameraWhiteBalanceBlue: 1648 70 | -------------------------------------------------------------------------------- /default/config_011509P0027/left_kinect2_color_qhd/gripperMask.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/default/config_011509P0027/left_kinect2_color_qhd/gripperMask.bmp -------------------------------------------------------------------------------- /default/config_011509P0027/left_kinect2_depth/gripperMask.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/default/config_011509P0027/left_kinect2_depth/gripperMask.bmp -------------------------------------------------------------------------------- /default/config_011509P0027/left_kinect2_ir/gripperMask.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/default/config_011509P0027/left_kinect2_ir/gripperMask.bmp -------------------------------------------------------------------------------- /default/config_011509P0027/right_hand_camera/cameraCalibration.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | savedTime: 3 | - 1.4855545476549854e+09 4 | hand_camera_offset: { px:4.0000000000000001e-02, 5 | py:1.4000000000000000e-02, pz:1.5890000000000001e-02, qw:1., qx:0., 6 | qy:0., qz:0. } 7 | cropUpperLeftCorner: 8 | - 320. 9 | - 200. 10 | mu: 11 | - 2.3999999999999998e-03 12 | - 2.4499999999999999e-03 13 | kappa: 14 | - 0. 15 | - 0. 16 | vanishingPointReticle: 17 | - 319. 18 | - 189. 19 | heightReticles: 20 | - 331. 21 | - 332. 22 | - 337. 23 | - 343. 24 | - 166. 25 | - 161. 26 | - 150. 27 | - 126. 28 | colorReticles: 29 | - 462 30 | - 450 31 | - 439 32 | - 428 33 | - 419 34 | - 410 35 | - 405 36 | - 399 37 | - 394 38 | - 389 39 | - 383 40 | - 381 41 | - 379 42 | - 378 43 | - 153 44 | - 153 45 | - 153 46 | - 153 47 | - 153 48 | - 154 49 | - 154 50 | - 154 51 | - 154 52 | - 154 53 | - 155 54 | - 155 55 | - 155 56 | - 155 57 | lensCorrections: 58 | - 9.3500000000000028e-01 59 | - 8.9500000000000024e-01 60 | - 8.9500000000000024e-01 61 | - 8.9500000000000024e-01 62 | - 1.0950000000000004e+00 63 | - 1.0550000000000004e+00 64 | - 1.0550000000000004e+00 65 | - 1.0550000000000004e+00 66 | gear0offset: 67 | - 2.3000000044703484e-02 68 | - 2.3000000044703484e-02 69 | - 1.6722800210118294e-02 70 | - 0. 71 | cameraExposure: 80 72 | cameraGain: 15 73 | cameraWhiteBalanceRed: 1124 74 | cameraWhiteBalanceGreen: 1024 75 | cameraWhiteBalanceBlue: 2048 76 | -------------------------------------------------------------------------------- /default/config_011509P0027/right_hand_camera/gripperMask.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/default/config_011509P0027/right_hand_camera/gripperMask.bmp -------------------------------------------------------------------------------- /default/config_011509P0030/config.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | savedTime: 3 | - 1.4713728395251875e+09 4 | currentTableZ: 5 | - 2.8427568509041745e-01 6 | -------------------------------------------------------------------------------- /default/config_011509P0030/left_hand_camera/cameraCalibration.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | savedTime: 3 | - 1.4713689968733110e+09 4 | hand_camera_offset: { px:3.8150000000000003e-02, 5 | py:1.1440000000000001e-02, pz:1.5890000000000001e-02, qw:1., qx:0., 6 | qy:0., qz:0. } 7 | cropUpperLeftCorner: 8 | - 320. 9 | - 200. 10 | vanishingPointReticle: 11 | - 309. 12 | - 163. 13 | heightReticles: 14 | - 354. 15 | - 358. 16 | - 359. 17 | - 367. 18 | - 158. 19 | - 151. 20 | - 142. 21 | - 120. 22 | colorReticles: 23 | - 462 24 | - 450 25 | - 439 26 | - 428 27 | - 419 28 | - 410 29 | - 405 30 | - 399 31 | - 394 32 | - 389 33 | - 383 34 | - 381 35 | - 379 36 | - 378 37 | - 153 38 | - 153 39 | - 153 40 | - 153 41 | - 153 42 | - 154 43 | - 154 44 | - 154 45 | - 154 46 | - 154 47 | - 155 48 | - 155 49 | - 155 50 | - 155 51 | lensCorrections: 52 | - 8.5500000000000020e-01 53 | - 8.1500000000000017e-01 54 | - 8.1500000000000017e-01 55 | - 8.1500000000000017e-01 56 | - 1.0550000000000004e+00 57 | - 1.0150000000000003e+00 58 | - 1.0150000000000003e+00 59 | - 1.0150000000000003e+00 60 | gear0offset: 61 | - 3.2000001519918442e-02 62 | - 2.6000000536441803e-02 63 | - 1.6722800210118294e-02 64 | - 0. 65 | cameraExposure: 80 66 | cameraGain: 30 67 | cameraWhiteBalanceRed: 1024 68 | cameraWhiteBalanceGreen: 1024 69 | cameraWhiteBalanceBlue: 2048 70 | -------------------------------------------------------------------------------- /default/config_011509P0030/left_hand_camera/gripperMask.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/default/config_011509P0030/left_hand_camera/gripperMask.bmp -------------------------------------------------------------------------------- /default/config_011509P0030/left_kinect2_color_qhd/cameraCalibration.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | savedTime: 3 | - 1.4713730792264817e+09 4 | hand_camera_offset: { px:8.9999999999999997e-02, 5 | py:1.2500000000000000e-01, pz:1.5890000000000001e-02, qw:1., qx:0., 6 | qy:0., qz:0. } 7 | cropUpperLeftCorner: 8 | - 320. 9 | - 200. 10 | vanishingPointReticle: 11 | - 309. 12 | - 163. 13 | heightReticles: 14 | - 324. 15 | - 326. 16 | - 330. 17 | - 338. 18 | - 140. 19 | - 133. 20 | - 124. 21 | - 105. 22 | colorReticles: 23 | - 462 24 | - 450 25 | - 439 26 | - 428 27 | - 419 28 | - 410 29 | - 405 30 | - 399 31 | - 394 32 | - 389 33 | - 383 34 | - 381 35 | - 379 36 | - 378 37 | - 153 38 | - 153 39 | - 153 40 | - 153 41 | - 153 42 | - 154 43 | - 154 44 | - 154 45 | - 154 46 | - 154 47 | - 155 48 | - 155 49 | - 155 50 | - 155 51 | lensCorrections: 52 | - 9.7500000000000031e-01 53 | - 9.3500000000000028e-01 54 | - 9.3500000000000028e-01 55 | - 9.3500000000000028e-01 56 | - 7.7500000000000013e-01 57 | - 7.3500000000000010e-01 58 | - 7.3500000000000010e-01 59 | - 7.3500000000000010e-01 60 | gear0offset: 61 | - 3.2000001519918442e-02 62 | - 2.6000000536441803e-02 63 | - 1.6722800210118294e-02 64 | - 0. 65 | cameraExposure: 80 66 | cameraGain: 5 67 | cameraWhiteBalanceRed: 1224 68 | cameraWhiteBalanceGreen: 1124 69 | cameraWhiteBalanceBlue: 1648 70 | -------------------------------------------------------------------------------- /default/config_011509P0030/left_kinect2_color_qhd/gripperMask.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/default/config_011509P0030/left_kinect2_color_qhd/gripperMask.bmp -------------------------------------------------------------------------------- /default/config_011509P0030/right_hand_camera/cameraCalibration.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | savedTime: 3 | - 1.5161250106642034e+09 4 | hand_camera_offset: { px:3.8150000000000003e-02, 5 | py:1.1440000000000001e-02, pz:1.5890000000000001e-02, qw:1., qx:0., 6 | qy:0., qz:0. } 7 | cropUpperLeftCorner: 8 | - 320. 9 | - 200. 10 | mu: 11 | - 2.3999999999999998e-03 12 | - 2.4499999999999999e-03 13 | kappa: 14 | - 0. 15 | - 0. 16 | vanishingPointReticle: 17 | - 309. 18 | - 189. 19 | heightReticles: 20 | - 335. 21 | - 339. 22 | - 345. 23 | - 356. 24 | - 161. 25 | - 157. 26 | - 145. 27 | - 124. 28 | colorReticles: 29 | - 462 30 | - 450 31 | - 439 32 | - 428 33 | - 419 34 | - 410 35 | - 405 36 | - 399 37 | - 394 38 | - 389 39 | - 383 40 | - 381 41 | - 379 42 | - 378 43 | - 153 44 | - 153 45 | - 153 46 | - 153 47 | - 153 48 | - 154 49 | - 154 50 | - 154 51 | - 154 52 | - 154 53 | - 155 54 | - 155 55 | - 155 56 | - 155 57 | lensCorrections: 58 | - 4.5499999999999990e-01 59 | - 4.5499999999999990e-01 60 | - 4.5499999999999990e-01 61 | - 4.5499999999999990e-01 62 | - 1.1350000000000005e+00 63 | - 1.1350000000000005e+00 64 | - 1.1350000000000005e+00 65 | - 1.1350000000000005e+00 66 | gear0offset: 67 | - 2.3000000044703484e-02 68 | - 2.3000000044703484e-02 69 | - 1.6722800210118294e-02 70 | - 0. 71 | cameraExposure: 80 72 | cameraGain: 30 73 | cameraWhiteBalanceRed: 1024 74 | cameraWhiteBalanceGreen: 1024 75 | cameraWhiteBalanceBlue: 2048 76 | -------------------------------------------------------------------------------- /default/config_011509P0030/right_hand_camera/gripperMask.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/default/config_011509P0030/right_hand_camera/gripperMask.bmp -------------------------------------------------------------------------------- /default/config_011601P0011/config.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | savedTime: 3 | - 1.4864946269073248e+09 4 | currentTableZ: 5 | - 1.9892797096180967e-01 6 | -------------------------------------------------------------------------------- /default/config_011601P0011/left_hand_camera/cameraCalibration.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | savedTime: 3 | - 1.5252722262330146e+09 4 | hand_camera_offset: { px:4.2500000000000003e-02, 5 | py:5.0000000000000001e-03, pz:1.5890000000000001e-02, qw:1., qx:0., 6 | qy:0., qz:0. } 7 | cropUpperLeftCorner: 8 | - 320. 9 | - 200. 10 | mu: 11 | - 2.3999999999999998e-03 12 | - 2.4499999999999999e-03 13 | kappa: 14 | - 0. 15 | - 0. 16 | vanishingPointReticle: 17 | - 309. 18 | - 189. 19 | heightReticles: 20 | - 318. 21 | - 319. 22 | - 326. 23 | - 338. 24 | - 174. 25 | - 170. 26 | - 155. 27 | - 137. 28 | colorReticles: 29 | - 462 30 | - 450 31 | - 439 32 | - 428 33 | - 419 34 | - 410 35 | - 405 36 | - 399 37 | - 394 38 | - 389 39 | - 383 40 | - 381 41 | - 379 42 | - 378 43 | - 153 44 | - 153 45 | - 153 46 | - 153 47 | - 153 48 | - 154 49 | - 154 50 | - 154 51 | - 154 52 | - 154 53 | - 155 54 | - 155 55 | - 155 56 | - 155 57 | lensCorrections: 58 | - 6.5500000000000003e-01 59 | - 6.5500000000000003e-01 60 | - 6.5500000000000003e-01 61 | - 6.5500000000000003e-01 62 | - 9.7500000000000031e-01 63 | - 9.3500000000000028e-01 64 | - 9.3500000000000028e-01 65 | - 9.3500000000000028e-01 66 | gear0offset: 67 | - 2.3000000044703484e-02 68 | - 2.3000000044703484e-02 69 | - 1.6722800210118294e-02 70 | - 0. 71 | cameraExposure: 100 72 | cameraGain: 11 73 | cameraWhiteBalanceRed: 1024 74 | cameraWhiteBalanceGreen: 1024 75 | cameraWhiteBalanceBlue: 2496 76 | -------------------------------------------------------------------------------- /default/config_011601P0011/left_hand_camera/gripperMask.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/default/config_011601P0011/left_hand_camera/gripperMask.bmp -------------------------------------------------------------------------------- /default/config_011601P0011/right_hand_camera/cameraCalibration.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | savedTime: 3 | - 1.5252154384622738e+09 4 | hand_camera_offset: { px:1.0000000000000000e-02, 5 | py:2.0000000000000000e-03, pz:0., qw:1., qx:0., qy:0., qz:0. } 6 | cropUpperLeftCorner: 7 | - 320. 8 | - 200. 9 | mu: 10 | - 2.3999999999999998e-03 11 | - 2.4499999999999999e-03 12 | kappa: 13 | - 0. 14 | - 0. 15 | vanishingPointReticle: 16 | - 320. 17 | - 200. 18 | heightReticles: 19 | - 320. 20 | - 324. 21 | - 327. 22 | - 334. 23 | - 200. 24 | - 192. 25 | - 180. 26 | - 160. 27 | colorReticles: 28 | - 462 29 | - 450 30 | - 439 31 | - 428 32 | - 419 33 | - 410 34 | - 405 35 | - 399 36 | - 394 37 | - 389 38 | - 383 39 | - 381 40 | - 379 41 | - 378 42 | - 153 43 | - 153 44 | - 153 45 | - 153 46 | - 153 47 | - 154 48 | - 154 49 | - 154 50 | - 154 51 | - 154 52 | - 155 53 | - 155 54 | - 155 55 | - 155 56 | lensCorrections: 57 | - 5.7499999999999996e-01 58 | - 5.3499999999999992e-01 59 | - 5.3499999999999992e-01 60 | - 5.3499999999999992e-01 61 | - 8.5500000000000020e-01 62 | - 8.1500000000000017e-01 63 | - 8.1500000000000017e-01 64 | - 8.1500000000000017e-01 65 | gear0offset: 66 | - 2.3000000044703484e-02 67 | - 2.3000000044703484e-02 68 | - 1.6722800210118294e-02 69 | - 0. 70 | cameraExposure: 80 71 | cameraGain: 20 72 | cameraWhiteBalanceRed: 1024 73 | cameraWhiteBalanceGreen: 1024 74 | cameraWhiteBalanceBlue: 2048 75 | -------------------------------------------------------------------------------- /default/config_011601P0011/right_hand_camera/gripperMask.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/default/config_011601P0011/right_hand_camera/gripperMask.bmp -------------------------------------------------------------------------------- /default/objects/baxterNarrowLongFinger/ein/3dGrasps/3dGrasps.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | grasps: 3 | size: 1 4 | graspPoses: 5 | - { px:-5.0942909524586463e-03, py:1.6204069979353558e-02, 6 | pz:-1.1333567824655510e-01, qw:9.9999994039535522e-01, qx:0., 7 | qy:0., qz:1.1641532182693481e-10, tries:1., successes:1., 8 | failures:0., jams:0. } 9 | placeUnderPoints: 10 | size: 0 11 | pupPoses: 12 | [] 13 | placeOverPoints: 14 | size: 0 15 | popPoses: 16 | [] 17 | -------------------------------------------------------------------------------- /default/objects/baxterNarrowLongFinger/ein/ir2d/ir2d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/default/objects/baxterNarrowLongFinger/ein/ir2d/ir2d.png -------------------------------------------------------------------------------- /default/objects/baxterNarrowLongFinger/ein/servoCrops/aerialHeight0Gradients.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | aerialHeight0Gradients: !!opencv-matrix 3 | rows: 1 4 | cols: 1 5 | dt: d 6 | data: [ 0. ] 7 | -------------------------------------------------------------------------------- /default/objects/baxterNarrowLongFinger/ein/servoCrops/aerialHeight1Gradients.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | aerialHeight1Gradients: !!opencv-matrix 3 | rows: 1 4 | cols: 1 5 | dt: d 6 | data: [ 0. ] 7 | -------------------------------------------------------------------------------- /default/objects/baxterNarrowLongFinger/ein/servoCrops/aerialHeight2Gradients.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | aerialHeight2Gradients: !!opencv-matrix 3 | rows: 1 4 | cols: 1 5 | dt: d 6 | data: [ 0. ] 7 | -------------------------------------------------------------------------------- /default/objects/baxterNarrowLongFinger/ein/servoCrops/aerialHeight3Gradients.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | aerialHeight3Gradients: !!opencv-matrix 3 | rows: 1 4 | cols: 1 5 | dt: d 6 | data: [ 0. ] 7 | -------------------------------------------------------------------------------- /default/objects/baxterNarrowLongFinger/ein/servoCrops/servoCrop0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/default/objects/baxterNarrowLongFinger/ein/servoCrops/servoCrop0.png -------------------------------------------------------------------------------- /default/objects/baxterNarrowLongFinger/ein/servoCrops/servoCrop1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/default/objects/baxterNarrowLongFinger/ein/servoCrops/servoCrop1.png -------------------------------------------------------------------------------- /default/objects/baxterNarrowLongFinger/ein/servoCrops/servoCrop2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/default/objects/baxterNarrowLongFinger/ein/servoCrops/servoCrop2.png -------------------------------------------------------------------------------- /default/objects/baxterNarrowLongFinger/ein/servoCrops/servoCrop3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/default/objects/baxterNarrowLongFinger/ein/servoCrops/servoCrop3.png -------------------------------------------------------------------------------- /default/objects/knn.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | features: !!opencv-matrix 3 | rows: 0 4 | cols: 0 5 | dt: u 6 | data: [] 7 | labels: !!opencv-matrix 8 | rows: 0 9 | cols: 0 10 | dt: u 11 | data: [] 12 | -------------------------------------------------------------------------------- /default/objects/labels.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | labels: 3 | - baxterNarrowLongFinger 4 | poseModels: 5 | - B 6 | -------------------------------------------------------------------------------- /default/simulator/sprites/brush/image.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/default/simulator/sprites/brush/image.ppm -------------------------------------------------------------------------------- /default/simulator/tableTile.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/default/simulator/tableTile.png -------------------------------------------------------------------------------- /doc/README: -------------------------------------------------------------------------------- 1 | 2 | sudo apt-get install ruby-dev ruby-mkrf ruby2.0 ruby2.0-dev nodejs 3 | 4 | Due to a bug in Ubuntu: 5 | sudo ln -sf /usr/bin/ruby2.0 /usr/bin/ruby 6 | sudo ln -sf /usr/bin/gem2.0 /usr/bin/gem 7 | 8 | Then: 9 | sudo gem install jekyll -v 2.5 10 | 11 | jekyll serve 12 | 13 | To check out the "official" release documentation at h2r.github.io/ein use: 14 | git clone http://github.com/h2r/ein ein-website 15 | cd ein-website 16 | git checkout gh-pages 17 | 18 | However the Ein doc is also duplicated in each checkout and branch so 19 | the way to see the latest doc is to use the doc directory in an ein 20 | checkout and view it by running Jeykll locally. 21 | 22 | We chose to use Jekyll because that is what github uses to host a 23 | documentation website. 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /doc/_config.yml: -------------------------------------------------------------------------------- 1 | # Site settings 2 | title: Ein 3 | #email: ein@lists.cs.brown.edu 4 | description: > # this means to ignore newlines until "baseurl:" 5 | Ein is a complete pick-and-place stack for Baxter. 6 | baseurl: "/ein" # the subpath of your site, e.g. /blog 7 | url: "http://h2r.github.io" # the base hostname & protocol for your site 8 | #twitter_username: h2r 9 | github_username: h2r 10 | 11 | # Build settings 12 | markdown: kramdown 13 | production_url : http://h2r.github.io/ein 14 | kramdown: 15 | input: GFM 16 | hard_wrap: false -------------------------------------------------------------------------------- /doc/_data/ein_version.yml: -------------------------------------------------------------------------------- 1 | 1.1 2 | -------------------------------------------------------------------------------- /doc/_includes/head.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | {% if page.title %}{{ page.title }}{% else %}{{ site.title }}{% endif %} 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /doc/_includes/header.html: -------------------------------------------------------------------------------- 1 | {% assign VERSION = File.read("/home/stefie10/dev/baxter-dev1.2.0/src/ein/doc/VERSION") %} 2 |
3 | 4 |
5 | 6 | {{ site.title }} {{ site.data.ein_version }} 7 | 8 | 27 | 28 |
29 | 30 |
31 | -------------------------------------------------------------------------------- /doc/_layouts/default.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | {% include head.html %} 5 | 6 | 7 | 8 | {% include header.html %} 9 | 10 |
11 |
12 | {{ content }} 13 |
14 |
15 | 16 | {% include footer.html %} 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /doc/_layouts/page.html: -------------------------------------------------------------------------------- 1 | --- 2 | layout: default 3 | --- 4 |
5 | 6 |
7 |

{{ page.title }}

8 |
9 | 10 |
11 | {{ content }} 12 |
13 | 14 |
15 | -------------------------------------------------------------------------------- /doc/_layouts/post.html: -------------------------------------------------------------------------------- 1 | --- 2 | layout: default 3 | --- 4 |
5 | 6 |
7 |

{{ page.title }}

8 |

{{ page.date | date: "%b %-d, %Y" }}{% if page.author %} • {{ page.author }}{% endif %}{% if page.meta %} • {{ page.meta }}{% endif %}

9 |
10 | 11 |
12 | {{ content }} 13 |
14 | 15 |
16 | -------------------------------------------------------------------------------- /doc/_posts/2016-06-10-welcome-to-ein.markdown: -------------------------------------------------------------------------------- 1 | --- 2 | layout: post 3 | title: "Welcome to Ein!" 4 | date: 2016-06-09 20:01:12 5 | categories: jekyll update 6 | --- 7 | 8 | 9 | Announcing the beta release of Ein! We can scan an object in 30 10 | seconds and pick is 100 times in a row using the Baxter wrist camera. -------------------------------------------------------------------------------- /doc/about.md: -------------------------------------------------------------------------------- 1 | --- 2 | layout: page 3 | title: About 4 | permalink: /about/ 5 | order: 200 6 | --- 7 | 8 | Ein was created and is maintained in the [Humans to Robots 9 | Laboratory](http://h2r.cs.brown.edu). 10 | -------------------------------------------------------------------------------- /doc/architecture.md: -------------------------------------------------------------------------------- 1 | --- 2 | layout: page 3 | title: System Architecture 4 | permalink: /architecture/ 5 | order: 7 6 | --- 7 | 8 | 9 | Ein is a C++ ROS program that does perception for robots. It is a C++ 10 | ROS node with an associated QT5 gui. When run, it opens up 11 | subscriptions to various perception topics. It starts a callback loop 12 | (timercallback1 in ein.cpp) that does the following things: 13 | 14 | * processes QT gui events 15 | * updates various GUI windows with the latest data (such as the wrist view image and current pose). 16 | * executes the word currently at the top of the callstack. There is a 17 | "stack collapse" mode where it may execute more than one word per 18 | cycle. This mode allows for increased computation speed because 19 | threads and gui events are not serviced, but at the expense of 20 | freezing message passing and the gui. 21 | * publishes a state message about the current status of Ein (current call stack, data stack, etc). 22 | * publishes control messages to the robot. Ein continuiesly drives 23 | the robot to assume the pose in `currentPose`. On Baxter, it runs 24 | inverse kinematics to find joint angles (using the service or 25 | IKFast), then publishes those joint angles to the robot to assume 26 | that pose. It also publishes commands to assume the state of the 27 | lights, the sonar, etc. These messages can be toggled with `zeroGMode` and `publishCommandsMode`. 28 | * it then renders some more things. 29 | 30 | Ein has an associated programming language, Back, that is used to 31 | instruct Ein to do things. Back is inspired by FORTH and RPL and is 32 | documented elsewhere on this website. Ein takes Back commands on a 33 | ROS topic and executes them. These commands can do anything from 34 | turning the lights on Baxter to engaging very high-level long-term 35 | actions. Ein is a ROS program and more information about its [ROS 36 | API](../ros/) is available. 37 | 38 | 39 | Each instance of Ein can control one or both arms on Baxter. However, 40 | because Ein is single threaded, if you use one Ein to control both 41 | arms, it will run twice as slow. As a result, we usually run two 42 | separate instances of Ein, one for each arm. The screen is set up 43 | accordingly with separate windows for each arm (windows 0 and 9). -------------------------------------------------------------------------------- /doc/assets/allenwrench.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/allenwrench.jpg -------------------------------------------------------------------------------- /doc/assets/allenwrench_density.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/allenwrench_density.jpg -------------------------------------------------------------------------------- /doc/assets/allenwrench_observed.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/allenwrench_observed.jpg -------------------------------------------------------------------------------- /doc/assets/baxter_crane.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/baxter_crane.jpg -------------------------------------------------------------------------------- /doc/assets/bg.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/bg.jpg -------------------------------------------------------------------------------- /doc/assets/bg_badgrippermask.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/bg_badgrippermask.jpg -------------------------------------------------------------------------------- /doc/assets/bg_discrepancy.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/bg_discrepancy.jpg -------------------------------------------------------------------------------- /doc/assets/bg_observed.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/bg_observed.jpg -------------------------------------------------------------------------------- /doc/assets/console_screenshot.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/console_screenshot.jpg -------------------------------------------------------------------------------- /doc/assets/einmainwindow_screenshot.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/einmainwindow_screenshot.jpg -------------------------------------------------------------------------------- /doc/assets/grippermask1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/grippermask1.jpg -------------------------------------------------------------------------------- /doc/assets/grippermask2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/grippermask2.jpg -------------------------------------------------------------------------------- /doc/assets/grippermask3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/grippermask3.jpg -------------------------------------------------------------------------------- /doc/assets/magic_circle.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/magic_circle.jpg -------------------------------------------------------------------------------- /doc/assets/narrow_finger_scene.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/narrow_finger_scene.jpg -------------------------------------------------------------------------------- /doc/assets/renderedwristview.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/renderedwristview.jpg -------------------------------------------------------------------------------- /doc/assets/renderedwristview_backwardscalibrated.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/renderedwristview_backwardscalibrated.jpg -------------------------------------------------------------------------------- /doc/assets/renderedwristview_calibrated1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/renderedwristview_calibrated1.jpg -------------------------------------------------------------------------------- /doc/assets/renderedwristview_calibrated2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/renderedwristview_calibrated2.jpg -------------------------------------------------------------------------------- /doc/assets/renderedwristview_calibrated3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/renderedwristview_calibrated3.jpg -------------------------------------------------------------------------------- /doc/assets/renderedwristview_calibrated4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/renderedwristview_calibrated4.jpg -------------------------------------------------------------------------------- /doc/assets/renderedwristview_nullcalibrated.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/renderedwristview_nullcalibrated.jpg -------------------------------------------------------------------------------- /doc/assets/wristview.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/wristview.jpg -------------------------------------------------------------------------------- /doc/assets/wristviewLegend.odg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/wristviewLegend.odg -------------------------------------------------------------------------------- /doc/assets/wristviewLegend.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/doc/assets/wristviewLegend.png -------------------------------------------------------------------------------- /doc/cameras.md: -------------------------------------------------------------------------------- 1 | --- 2 | layout: page 3 | title: Cameras 4 | permalink: cameras/ 5 | order: 20 6 | --- 7 | 8 | Ein supports any number of cameras attached to an arm. Each camera 9 | has associated calibration parameters, a stream buffer, and can be 10 | used to create synthetic photographs. You can see `kinect2.back` for 11 | example commands that add the Kinect 2 as an additional camera. By 12 | default, the Baxter wrist camera is configured for each arm, but if 13 | you mount other cameras on the arm, they can also be used to observe 14 | the scene. 15 | 16 | The word `cameraCreate` adds a camera, specifying its ROS image topic, 17 | the name for the camera, and its TF link. 18 | 19 | 20 | There is a focused camera which represents the current active camerea. 21 | The variable `cameraName` has the name of the current focused camera. 22 | You can run `incrementCamera` to change the camera and it will change 23 | what is viewed in the wrist view, the stream buffer, etc. 24 | 25 | For the Kinect 2, each image stream is treated as a separate camera in 26 | Ein: the RGB camera, the IR camera, and the depth return. In the 27 | cameras class, `cam_img` contains the raw image as received from a ROS 28 | topic (grayscale if that is what you got from the camera.) 29 | `cam_bgr_img` contains the image converted to bgr (even if it started 30 | out grayscale), and `cam_ycrcb_img` contains the image as ycrcb. -------------------------------------------------------------------------------- /doc/css/main.scss: -------------------------------------------------------------------------------- 1 | --- 2 | # Only the main Sass file needs front matter (the dashes are enough) 3 | --- 4 | @charset "utf-8"; 5 | 6 | 7 | 8 | // Our variables 9 | $base-font-family: Helvetica, Arial, sans-serif; 10 | $base-font-size: 16px; 11 | $small-font-size: $base-font-size * 0.875; 12 | $base-line-height: 1.5; 13 | 14 | $spacing-unit: 30px; 15 | 16 | $text-color: #111; 17 | $background-color: #fdfdfd; 18 | $brand-color: #2a7ae2; 19 | 20 | $grey-color: #828282; 21 | $grey-color-light: lighten($grey-color, 40%); 22 | $grey-color-dark: darken($grey-color, 25%); 23 | 24 | // Width of the content area 25 | $content-width: 800px; 26 | 27 | $on-palm: 600px; 28 | $on-laptop: 800px; 29 | 30 | 31 | 32 | // Using media queries with like this: 33 | // @include media-query($on-palm) { 34 | // .wrapper { 35 | // padding-right: $spacing-unit / 2; 36 | // padding-left: $spacing-unit / 2; 37 | // } 38 | // } 39 | @mixin media-query($device) { 40 | @media screen and (max-width: $device) { 41 | @content; 42 | } 43 | } 44 | 45 | 46 | 47 | // Import partials from `sass_dir` (defaults to `_sass`) 48 | @import 49 | "base", 50 | "layout", 51 | "syntax-highlighting" 52 | ; 53 | -------------------------------------------------------------------------------- /doc/data.md: -------------------------------------------------------------------------------- 1 | --- 2 | layout: page 3 | title: Data 4 | permalink: /data/ 5 | order: 7 6 | --- 7 | 8 | This page describes how to save and load data with Ein, especially 9 | focusing on calibrated light rays. One understandig of Ein is as a 10 | tool for collecting calibrated light rays. 11 | 12 | 13 | * Gripper mask 14 | * Camera calibration parameters 15 | * Camera color information 16 | * table height 17 | * hand camera offset? 18 | 19 | 20 | ### Saving and Loading Scenes 21 | 22 | 23 | 24 | ### Data Formats 25 | 26 | Ein stores its data in the `default` directory in the ein checkout. 27 | Configuration is stored in a directory named for the serial number. 28 | Objects are stored in the `objects` directory and gaussian maps are 29 | stored in the `maps` directory. Streams are stored in the `streams` 30 | directory. We use a combination of custom YAML formats as well as 31 | standard images to store data. Scenes consist of uuencoded compressed 32 | YAML, and we provide C++ and python code to unpack it into opencv 33 | images. 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /doc/feed.xml: -------------------------------------------------------------------------------- 1 | --- 2 | layout: null 3 | --- 4 | 5 | 6 | 7 | {{ site.title | xml_escape }} 8 | {{ site.description | xml_escape }} 9 | {{ site.url }}{{ site.baseurl }}/ 10 | 11 | {{ site.time | date_to_rfc822 }} 12 | {{ site.time | date_to_rfc822 }} 13 | Jekyll v{{ jekyll.version }} 14 | {% for post in site.posts limit:10 %} 15 | 16 | {{ post.title | xml_escape }} 17 | {{ post.content | xml_escape }} 18 | {{ post.date | date_to_rfc822 }} 19 | {{ post.url | prepend: site.baseurl | prepend: site.url }} 20 | {{ post.url | prepend: site.baseurl | prepend: site.url }} 21 | {% for tag in post.tags %} 22 | {{ tag | xml_escape }} 23 | {% endfor %} 24 | {% for cat in post.categories %} 25 | {{ cat | xml_escape }} 26 | {% endfor %} 27 | 28 | {% endfor %} 29 | 30 | 31 | -------------------------------------------------------------------------------- /doc/index.html: -------------------------------------------------------------------------------- 1 | --- 2 | layout: default 3 | --- 4 | 5 |
6 | 7 | Ein is software for pick-and-place for Baxter. It can map one side of 8 | an object with Baxter's wrist camera in about thirty seconds, then use 9 | that model to detect the object, localize it to within 2mm and pick it 10 | up. You can also use Baxter to automatically process a pile of 11 | objects, as depicted in our award-winning video. We are soliciting 12 | participation from all of the Baxters in the world to automatically 13 | process as many objects as possible. Learn more and sign up at 14 | the Million 15 | Object Challenge! 16 | 17 | 18 | 19 |

Posts

20 | 21 | 32 | 33 |

subscribe via RSS

34 | 35 |
36 | -------------------------------------------------------------------------------- /doc/lightfields.md: -------------------------------------------------------------------------------- 1 | --- 2 | layout: page 3 | title: Light Fields 4 | permalink: /lightfields/ 5 | order: 3 6 | --- 7 | 8 | 9 | Now we briefly describe how to use light fields. To set a focus sweep 10 | with 3d reconstruction, change `tableReconstructWithZTable1D` to set 11 | tableReconstructWithZTotalZDefault to a non-zero value. It will 12 | reconstruct in increments of the ...DeltaZ parameter up to the height 13 | of the gripper. 14 | 15 | To set the -------------------------------------------------------------------------------- /doc/pose.md: -------------------------------------------------------------------------------- 1 | --- 2 | layout: page 3 | title: Poses 4 | permalink: /pose/ 5 | order: 7 6 | --- 7 | 8 | 9 | Ein supports pose words, for example `currentPose`, which contains the 10 | targetted pose for the end effector, and `truePose`, which contains 11 | the last return for the real pose of the end effector. It also 12 | contains words for pose getters and setters, such as `setEEPose*` and `eePose*`, 13 | which returns and sets the px, py, pz (position) and qx, qy, qz, qw (quaternion) 14 | for the pose values. 15 | 16 | For example, 17 | 18 | `trueEEPose eePosePX` pushes the double value of the x position 19 | coordinate of the end effector. 20 | 21 | 22 | `currentEEPose 1 setEEPosePx` pushes a new pose on the stack whose px 23 | value is the double value `1`. 24 | 25 | `moveEEToPoseWord` moves the end effector. So a workflow for many 26 | applications is to create poses somehow (from object locations; from 27 | annotations) and then send the end effector to these poses. 28 | 29 | #### Exercise: Wave! 30 | 31 | Write a program that makes Baxter wave, by moving back and forth from 32 | the current starting pose. So the robot should move repeatedly back 33 | and forth relative to its starting position. You will need to use the 34 | `truePose`, as well as the getters and setters above. You may also 35 | find the `dup` word useful, which duplicates the current value at the 36 | top of the stack, `replicateWord` to repeat an action more than once, 37 | and `store` to store pose words as variables. 38 | 39 | Select to see our answer: 40 | 41 | 42 | truePose "x" store
43 | x dup eePosePX 0.1 + setEEPosePX "y" store
44 | ( x moveEeToPoseWord waitUntilAtCurrentPosition y moveEeToPoseWord waitUntilAtCurrentPosition ) 5 replicateWord 45 | {: style="color:white;" } 46 | 47 | 48 | -------------------------------------------------------------------------------- /doc/ros.md: -------------------------------------------------------------------------------- 1 | --- 2 | layout: page 3 | title: ROS 4 | permalink: /ros/ 5 | order: 7 6 | --- 7 | 8 | Ein integrates with ROS (Robot Operating System). It takes input on 9 | feeds such as ROS Image topics. It outputs on standard ROS topics 10 | such as 11 | [object_recognition_msgs](http://wiki.ros.org/object_recognition_msgs). 12 | 13 | Ein does not have to use ROS; indeed one of the first robots we 14 | integrated it with besides Baxter was the Aibo, and it connects over a 15 | TCP/IP connection. However data structures such as ros::Time and the 16 | like are embedded into Ein's code, so it must have a ROS environment 17 | to compile, currently ROS Indigo. 18 | 19 | 20 | ### Sending Commands to Ein 21 | 22 | You can send commands to Ein by publishing on the topic 23 | `/ein/right/forth_commands` or `/ein/left/forth_commands` for each 24 | hand. These topics take a std_msgs/String which is a valid Back 25 | program. You can use any ROS program to publish to this topic, 26 | including `rostopic pub`. For an example, see `sendToBothArms.sh` 27 | which sends commands to each arm. 28 | 29 | 30 | 31 | Document color calibration 32 | 33 | 34 | Write about the file system and where objects are stored 35 | 36 | which one is focused? 37 | 38 | setTargetClass (but change it to focused class) 39 | 40 | Write a word to map the whole table 41 | 42 | 43 | tableMap 44 | 45 | add pictures of predicted map 46 | 47 | 48 | scenePredictBestObject tableUpdateMaps 49 | 50 | 51 | 52 | To add objects to a map: 53 | 54 | tableTakeScene (move the arm to take the map) 55 | scenePredictBestObject (predict the best object) 56 | tableUpdateMaps (visualize what you predicted) 57 | 58 | 59 | (random things I don't understand that seem to result in a recognized object array being published) 60 | 0 sceneMapSceneObject 61 | recordPreTargetLock 62 | recordPostTargetLock 63 | publishRecognizedObjectArrayFromBlueBoxMemory (rename this) 64 | 65 | Topic: /ein_right/blue_memory_objects 66 | 67 | 68 | Bug: 69 | predict orientation in recognized object array 70 | 71 | 72 | Write about Ein's architecture (ros, not ros, qt, everything else) 73 | -------------------------------------------------------------------------------- /doc/scanningobjects.md: -------------------------------------------------------------------------------- 1 | --- 2 | layout: page 3 | title: Mapping Objects 4 | permalink: /scanningobjects/ 5 | order: 4 6 | --- 7 | 8 | We will set up infinite scan so the robot automatically learns about 9 | objects. This process is a giant for loop over objects; in the inner 10 | loop you can do whatever experiments you like. 11 | 12 | 13 | There are three poses defined in scan.back designated with functionality for the scan: 14 | 15 | ``` 16 | inputPileWorkspace 17 | playWorkspace 18 | outputPileWorkspace 19 | ``` 20 | Move the arm to these locations by issuing, for instance: 21 | 22 | ``` 23 | playWorkspace moveEeToPoseWord 24 | ``` 25 | You can change the values of these variables in the file, save, and reload with 26 | 27 | ``` 28 | "scan" import 29 | ``` 30 | 31 | Check the `inputPile` and `playWorkspace` and make sure there is about 20cm by 20cm of free real estate. The output 32 | pile doesn't really need space as long as objects deposited there can clear the other workspaces. 33 | 34 | Once everything is clear, issue 35 | 36 | ``` 37 | catScan5CacheBackgrounds 38 | ``` 39 | 40 | This word invokes scan patterns which cover the default workspaces 41 | slightly better than a square scan. If you change the workspaces you 42 | might need to change the scan patterns. 43 | 44 | Check the background maps by calling 45 | 46 | ``` 47 | catScan5LoadInputBg 48 | catScan5LoadPlayBg 49 | ``` 50 | and checking the Background View window after each command. 51 | 52 | 53 | Find three objects that fit in the 4cm gripper and contrast with the background. Place them in the `inputPile` workspace and issue 54 | 55 | ``` 56 | tableInfiniteScan 57 | ``` 58 | 59 | The arm should scan the play workspace to make a new map, scan the input workspace to find an object, move the object to the the play workspace, 60 | play with it, move it to the output pile, and begin again by scanning the play workspace. 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /doc/scene.md: -------------------------------------------------------------------------------- 1 | --- 2 | layout: page 3 | title: Synthetic Photographs 4 | permalink: /scene/ 5 | order: 7 6 | --- 7 | 8 | This page describes how to use the synthetic photograph infrastructure 9 | with Ein. 10 | 11 | A Gaussian Map is a synthetic photograph of a scene. It consists of 12 | an MxN matrix of cells, where each cell contains an RGB mean and 13 | variance estimate, as well as a height estimate (z). It also has an 14 | associated anchor pose representing the location of the map. 15 | 16 | A Scene is an object consisting of three Gaussian Maps: the background 17 | map, the observed map, and the predicted map, as well as a pose for 18 | the scene. `currentScene` pushes the current scene on the stack. 19 | `scenePredictedMap`, `sceneObservedMap` and `sceneBackgroundMap` 20 | access the various maps and push them on the stack. By default, all 21 | maps are pointers. Memory is not reallocated when they are moved 22 | around on the stack. Similarly, if you save the current scene as a 23 | variable, and then change the current scene, the variable will also 24 | change as it is a pointer to the current scene. To perform a deep 25 | copy, use `sceneCopyScene` and `sceneCopyGaussianMap`. 26 | 27 | To save and load gaussian maps from file, use `sceneSaveGaussianMap` 28 | and `sceneLoadGaussianMap`. 29 | 30 | #### Exercise: Save multiple background maps. 31 | 32 | Make two different maps for different parts of the workspace, and save 33 | them both to disk as bg1 and bg2. Load them both into variables and 34 | alternate which one is the focused background map. You can use 35 | `tableShortTakeScene` to create an observed map with just a few frames 36 | of video at the current position to speed up testing. Note that if 37 | you set the background map in the current scene, it will not be 38 | visible until after calling `sceneRenderScene`. 39 | 40 | Select to see our program! 41 | 42 | tableShortTakeScene currentObservedMap "bg1" sceneSaveGaussianMap
43 | ( xUp ) 10 replicateWord waitUntilAtCurrentPosition tableShortTakeScene currentObservedMap "bg2" sceneSaveGaussianMap
44 | "bg1" sceneLoadGaussianMap "bg1" store
45 | "bg2" sceneLoadGaussianMap "bg2" store
46 | currentScene bg1 sceneSetBackgroundMap sceneRenderScene
47 | currentScene bg2 sceneSetBackgroundMap sceneRenderScene
48 | {: style="color:white;" } 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /doc/simulation.md: -------------------------------------------------------------------------------- 1 | --- 2 | layout: page 3 | title: Simulation Mode 4 | permalink: /simulation/ 5 | order: 7 6 | --- 7 | 8 | Ein has a simulation mode that allows it to run without being 9 | connected to any robot. This mode can be used to view data from the 10 | Million Object Challenge and render photographs. To start Ein in 11 | simulation mode, first change "physical" to "simulated" in the command 12 | line arguments. This will cause Ein to start with fake sensor data 13 | from Baxter. It will generate fake camera images and poses so that 14 | various parts of the system will not crash. 15 | 16 | Here is an example: `catkin_make && gdb --args ./devel/lib/ein/ein physical left`. 17 | 18 | You will need to start your own ROS core: `roscore` in another 19 | terminal, and make sure that your ROS_MASTER_URI is set appropriately. 20 | If you run the screen session with no arguments, it will default to 21 | localhost so everything should work. If you encounter problems, most 22 | likely your $ROS_MASTER_URI or $ROST_HOSTNAME or $ROS_IP are set 23 | incorrectly. 24 | -------------------------------------------------------------------------------- /doc/words.md: -------------------------------------------------------------------------------- 1 | --- 2 | layout: page 3 | title: Word Index 4 | permalink: /words/ 5 | order: 99 6 | --- 7 | 8 | This page contains all the words in Ein and their help text. You can 9 | access this within Ein by running
`( word ) help`. Note that 10 | different compiles of Ein (for different robots) will contain 11 | different words. For example, if you have compiled Ein for Jaco, then 12 | the Baxter-specific words will not appear in this index, and 13 | vice-versa if you have compiled Ein for Baxter and not Jaco. 14 | 15 | 31 | 32 | {% include ein_words.html %} 33 | -------------------------------------------------------------------------------- /ein_aibo.screenrc: -------------------------------------------------------------------------------- 1 | # 2 | # various settings suggested by other users 3 | # 4 | # derived from the Paradox pair programming screenrc, UChicago 2010 5 | # ~ yes, we are all total newbs ~ 6 | # 7 | 8 | startup_message off 9 | 10 | 11 | defscrollback 5000 12 | 13 | escape `` 14 | 15 | # this hard status was stolen from the net, all credit goes to its originator 16 | hardstatus alwayslastline 17 | #hardstatus string '%{= mK}%-Lw%{= KW}%50>%n%f* %t%{= mK}%+Lw%< %{= kG}%-=%D %d %M %Y %c:%s%{-}' 18 | hardstatus string '%{= kG}[ %{G}%H %{g}][%= %{= kw}%?%-Lw%?%{r}(%{W}%n*%f%t%?(%u)%?%{r})%{w}%?%+Lw%?%?%= %{g}][%{B} %d/%m %{W}%c %{g}]' 19 | 20 | # A hint for using multiuser mode: make sure that both parties have identically 21 | # sized terminals, otherwise there could be graphical mismatches and undrawn text 22 | # for one party :( . 23 | 24 | # turn multiuser mode on so others can connect. 25 | multiuser on 26 | 27 | # add the specified user to this screen session so that they can connect. 28 | # the initiating user is allowed automatically, but add both so either can 29 | # run this script without modification. 30 | 31 | # Starts a new screen with title " MAKE" 32 | # this is basically equivalent to pressing the following from within screen: 33 | # `c (starts a new window) 34 | # `:title " MAKE" (sets the title to " MAKE") 35 | #screen -t " MAKE" 36 | 37 | #stuff "unset STY " 38 | #stuff "screen " 39 | 40 | screen -t " ein" 41 | stuff "source ./setup.sh " 42 | stuff "catkin_make && gdb --args ./devel/lib/ein/ein physical right gui" 43 | screen -t " replR" 44 | stuff "ARM=right screen -m -c src/ein/ein_client.screenrc " 45 | screen -t " free" 46 | stuff "source ./setup.sh " 47 | screen -t " vim" 48 | stuff "source ./setup.sh " 49 | stuff "cd src/ein/src " 50 | stuff "vim -p ein.cpp ein.h config.h gaussian_map.cpp gaussian_map.h ../back/scene.back ../back/scan.back " 51 | screen -t " git" 52 | stuff "source ./setup.sh " 53 | stuff "cd $(rospack find ein) " 54 | screen -t " emacs" 55 | stuff "source ./setup.sh " 56 | stuff "cd $(rospack find ein) " 57 | stuff "emacs -nw src/ein.cpp " 58 | 59 | screen -t " state publisher" 60 | stuff "source ./setup.sh " 61 | stuff "roslaunch ein aibo.launch" 62 | 63 | 64 | screen -t " roscore" 65 | stuff "source ./setup.sh " 66 | stuff "roscore " 67 | 68 | screen -t " website" 69 | stuff "cd src/ein/doc && jekyll serve" 70 | 71 | -------------------------------------------------------------------------------- /ein_baxter.vimrc: -------------------------------------------------------------------------------- 1 | 2 | " Commented lines start with " as you can see. 3 | " Syntax hilighting is absolutely essential. 4 | syntax on 5 | 6 | 7 | " This adjusts the tab structure. It is functional for me but probably 8 | " needs to be fixed in order to behave ENTIRELY correctly. 9 | set shiftwidth=2 10 | set softtabstop=2 11 | 12 | " Display line numbers. 13 | set nu 14 | 15 | " This option hilights your searches. To undo this after a search, type :nohls 16 | set hlsearch 17 | 18 | let g:lisp_rainbow = 1 19 | 20 | "colorscheme custom 21 | "colorscheme nightflight 22 | 23 | au BufNewFile,BufRead *.back set syntax=lisp 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /ein_client.screenrc: -------------------------------------------------------------------------------- 1 | # configure the client window to show the stack, the standard out, and the standard repl. 2 | autodetach off 3 | escape `` 4 | 5 | 6 | split 7 | split -v 8 | 9 | screen -t " state" 10 | stuff "source setup.sh " 11 | stuff "rosrun ein ein_print_state.py $ARM " 12 | 13 | split -v 14 | focus 15 | screen -t " call_stack" 16 | stuff "source setup.sh " 17 | stuff "rosrun ein ein_print_stack.py $ARM call " 18 | 19 | split -v 20 | focus 21 | screen -t " data_stack" 22 | stuff "source setup.sh " 23 | stuff "rosrun ein ein_print_stack.py $ARM data " 24 | 25 | 26 | focus 27 | screen -t " console" 28 | stuff "source setup.sh " 29 | stuff "rosrun ein ein_print_console.py $ARM " 30 | 31 | 32 | focus 33 | screen -t " repl" 34 | stuff "source setup.sh " 35 | stuff "rosrun ein ein_client.py --silence-console-messages --silence-stacks $ARM " 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /ein_kuka.screenrc: -------------------------------------------------------------------------------- 1 | startup_message off 2 | 3 | defscrollback 5000 4 | 5 | escape `` 6 | 7 | hardstatus alwayslastline 8 | hardstatus string '%{= kG}[ %{G}%H %{g}][%= %{= kw}%?%-Lw%?%{r}(%{W}%n*%f%t%?(%u)%?%{r})%{w}%?%+Lw%?%?%= %{g}][%{B} %d/%m %{W}%c %{g}]' 9 | 10 | screen -t " roscore" 11 | stuff "source ./setup.sh " 12 | stuff "roscore " 13 | 14 | 15 | screen -t " einR" 16 | stuff "source ./setup.sh " 17 | stuff "catkin_make && gdb --args ./devel/lib/ein/ein physical right gui" 18 | screen -t " replR" 19 | stuff "ARM=right screen -m -c src/ein/ein_client.screenrc " 20 | 21 | screen -t " rviz" 22 | stuff "source ./setup.sh " 23 | stuff "rosrun rviz rviz" 24 | 25 | screen -t " statepub" 26 | stuff "source ./setup.sh " 27 | stuff "roslaunch iiwa_robotiq_description iiwa7_robotiq_upload.launch && roslaunch iiwa_robotiq_bringup iiwa7_robotiq_bringup.launch" 28 | 29 | screen -t " emacs" 30 | stuff "source ./setup.sh " 31 | stuff "cd $(rospack find ein) " 32 | stuff "emacs -nw src/ein.cpp " 33 | 34 | screen -t " vim" 35 | stuff "source ./setup.sh " 36 | stuff "cd src/ein/src " 37 | stuff "vim " 38 | screen -t " git" 39 | stuff "source ./setup.sh " 40 | stuff "cd $(rospack find ein) " 41 | 42 | 43 | screen -t " website" 44 | stuff "cd src/ein/doc && jekyll serve" 45 | -------------------------------------------------------------------------------- /ein_movo.screenrc: -------------------------------------------------------------------------------- 1 | startup_message off 2 | 3 | defscrollback 5000 4 | 5 | escape `` 6 | 7 | hardstatus alwayslastline 8 | hardstatus string '%{= kG}[ %{G}%H %{g}][%= %{= kw}%?%-Lw%?%{r}(%{W}%n*%f%t%?(%u)%?%{r})%{w}%?%+Lw%?%?%= %{g}][%{B} %d/%m %{W}%c %{g}]' 9 | 10 | screen -t " einR" 11 | stuff "source ./setup.sh " 12 | stuff "catkin_make && gdb --args ./devel/lib/ein/ein physical right gui" 13 | screen -t " replR" 14 | stuff "ARM=right screen -m -c src/ein/ein_client.screenrc " 15 | 16 | screen -t " gmapping mapping" 17 | stuff "source ./setup.sh " 18 | stuff "roslaunch movo_demos mapping.launch" 19 | 20 | 21 | screen -t " gmapping nav" 22 | stuff "source ./setup.sh " 23 | stuff "roslaunch movo_demos map_nav.launch map_file:=115.map" 24 | 25 | screen -t " rviz" 26 | stuff "source ./setup.sh " 27 | stuff "roslaunch movo_viz view_robot.launch function:=map_nav" 28 | 29 | 30 | screen -t " free" 31 | stuff "source ./setup.sh " 32 | 33 | screen -t " emacs" 34 | stuff "source ./setup.sh " 35 | stuff "cd $(rospack find ein) " 36 | stuff "emacs -nw src/ein.cpp " 37 | 38 | screen -t " vim" 39 | stuff "source ./setup.sh " 40 | stuff "cd src/ein/src " 41 | stuff "vim -p ein.cpp ein.h config.h gaussian_map.cpp gaussian_map.h ../back/scene.back ../back/scan.back " 42 | screen -t " git" 43 | stuff "source ./setup.sh " 44 | stuff "cd $(rospack find ein) " 45 | 46 | 47 | screen -t " website" 48 | stuff "cd src/ein/doc && jekyll serve" 49 | -------------------------------------------------------------------------------- /ein_pidrone.screenrc: -------------------------------------------------------------------------------- 1 | startup_message off 2 | 3 | defscrollback 5000 4 | 5 | escape `` 6 | 7 | hardstatus alwayslastline 8 | hardstatus string '%{= kG}[ %{G}%H %{g}][%= %{= kw}%?%-Lw%?%{r}(%{W}%n*%f%t%?(%u)%?%{r})%{w}%?%+Lw%?%?%= %{g}][%{B} %d/%m %{W}%c %{g}]' 9 | 10 | multiuser on 11 | 12 | screen -t " einR" 13 | stuff "source ./setup.sh " 14 | stuff "catkin_make && gdb --args ./devel/lib/ein/ein physical right gui" 15 | screen -t " replR" 16 | stuff "source ./setup.sh " 17 | stuff "ARM=right screen -m -c src/ein/ein_client.screenrc " 18 | screen -t " free" 19 | stuff "source ./setup.sh " 20 | screen -t " vim" 21 | stuff "source ./setup.sh " 22 | stuff "cd $(rospack find ein)/src " 23 | stuff "vim -p ein.cpp ein.h config.h gaussian_map.cpp gaussian_map.h ../back/scene.back ../back/scan.back " 24 | screen -t " git" 25 | stuff "source ./setup.sh " 26 | stuff "cd $(rospack find ein)/src " 27 | screen -t " emacs" 28 | stuff "source ./setup.sh " 29 | stuff "cd $(rospack find ein) " 30 | stuff "emacs -nw src/ein.cpp " 31 | screen -t " free" 32 | stuff "source ./setup.sh " 33 | screen -t " website" 34 | stuff "source ./setup.sh " 35 | stuff "cd src/ein/doc && jekyll serve" 36 | 37 | -------------------------------------------------------------------------------- /how-to-release.txt: -------------------------------------------------------------------------------- 1 | It's a good idea to run exportDoc to create a new version of the words 2 | table for the documentation page, and then copy ein_words.html to the 3 | doc/_includes directory and commit first. 4 | 5 | First create a new release branch from develop: 6 | $ git checkout -b release develop 7 | 8 | Then run: 9 | ./bump-version.sh and choose the new version number. This program will git commit, tag, and push the release. 10 | 11 | Finally run: 12 | git checkout master 13 | git merge --no-ff release 14 | 15 | and push. 16 | 17 | Ein is now released. 18 | 19 | The VERSION in develop is always the last version number we branched off of for the new official release. 20 | 21 | 22 | Copy the docs directory to the gh-pages branch to update the live website. 23 | 24 | When you go back to develop, make sure you git merge master, to pull down the version numbering changes, and anything you did on the release branch. 25 | 26 | 27 | -------------------------------------------------------------------------------- /images/black.tif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/images/black.tif -------------------------------------------------------------------------------- /images/calibration/magicpaper.odg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/images/calibration/magicpaper.odg -------------------------------------------------------------------------------- /images/calibration/magicpaper.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/images/calibration/magicpaper.pdf -------------------------------------------------------------------------------- /images/calibration/magicpaper.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/images/calibration/magicpaper.png -------------------------------------------------------------------------------- /images/gaze30006.tif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/images/gaze30006.tif -------------------------------------------------------------------------------- /images/gaze30007.tif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/images/gaze30007.tif -------------------------------------------------------------------------------- /images/gaze30008.tif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/images/gaze30008.tif -------------------------------------------------------------------------------- /images/gaze30009.tif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/images/gaze30009.tif -------------------------------------------------------------------------------- /images/gaze30010.tif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/images/gaze30010.tif -------------------------------------------------------------------------------- /images/gaze30011.tif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/images/gaze30011.tif -------------------------------------------------------------------------------- /images/gaze30012.tif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/images/gaze30012.tif -------------------------------------------------------------------------------- /images/ursula_neutral.tif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/images/ursula_neutral.tif -------------------------------------------------------------------------------- /images/ursula_no.tif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/images/ursula_no.tif -------------------------------------------------------------------------------- /images/ursula_yes.tif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/images/ursula_yes.tif -------------------------------------------------------------------------------- /launch/aibo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | "" 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/aibo.launch.bk: -------------------------------------------------------------------------------- 1 | 2 | 50 3 | "/base_link" 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /msg/EinConsole.msg: -------------------------------------------------------------------------------- 1 | string msg -------------------------------------------------------------------------------- /msg/EinState.msg: -------------------------------------------------------------------------------- 1 | # the entire relevant state of the ein system 2 | # please see config.h for details 3 | 4 | int32 zero_g 5 | 6 | int32 movement_state 7 | 8 | int32 patrol_state 9 | 10 | int32 place_mode 11 | 12 | int32 patrol_mode 13 | 14 | int32 idle_mode 15 | 16 | object_recognition_msgs/RecognizedObject[] objects 17 | 18 | string[] call_stack 19 | 20 | string[] data_stack 21 | 22 | string[] words 23 | 24 | string state_string 25 | -------------------------------------------------------------------------------- /python/ein_print_console.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | 4 | from ein.msg import EinConsole 5 | import os 6 | class EinPrint: 7 | def __init__(self, topic): 8 | 9 | self.state_subscriber = rospy.Subscriber(topic, 10 | EinConsole, self.einconsole_callback) 11 | self.state = None 12 | self.call_stack = [] 13 | self.data_stack = [] 14 | 15 | 16 | def einconsole_callback(self, msg): 17 | print str(msg.msg) 18 | 19 | def spin(self): 20 | while not rospy.is_shutdown(): 21 | rospy.sleep(0.2) 22 | 23 | def hangup(signal, stackframe): 24 | import signal 25 | import os 26 | os.kill(os.getpid(), signal.SIGTERM) 27 | 28 | 29 | def main(): 30 | import sys 31 | import signal 32 | 33 | if (len(sys.argv) != 2): 34 | print "usage: ein_print_console.py left|right" 35 | return 36 | 37 | arm = sys.argv[1] 38 | signal.signal(signal.SIGHUP, hangup) 39 | rospy.init_node("ein_print_console_%s" % arm, anonymous=True) 40 | 41 | rows, cols = os.popen('stty size', 'r').read().split() 42 | rows = int(rows) 43 | print "".rjust(rows, "\n") 44 | 45 | client = EinPrint("/ein/%s/console" % arm) 46 | client.spin() 47 | 48 | 49 | 50 | if __name__=='__main__': 51 | main() 52 | -------------------------------------------------------------------------------- /python/ein_print_stack.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | 4 | from ein.msg import EinState 5 | import os 6 | 7 | 8 | class EinStack: 9 | def __init__(self, state_topic, stack_to_use): 10 | print "state topic: ", state_topic 11 | 12 | self.state_subscriber = rospy.Subscriber(state_topic, 13 | EinState, self.state_callback) 14 | self.state = None 15 | self.call_stack = [] 16 | self.data_stack = [] 17 | self.stack_to_use = stack_to_use 18 | 19 | def state_callback(self, msg): 20 | #print "received state." 21 | self.state = msg 22 | self.call_stack = self.state.call_stack 23 | self.data_stack = self.state.data_stack 24 | 25 | def printStack(self, stack): 26 | rows, cols = os.popen('stty size', 'r').read().split() 27 | rows = int(rows) 28 | print "rows", rows 29 | stack_to_print = stack[0:rows] 30 | string = "" 31 | if len(stack_to_print) != rows: 32 | string += "".rjust(rows - len(stack_to_print), "\n") 33 | for i, word in enumerate(stack_to_print): 34 | string += "%s: %s\n" % (str(len(stack_to_print) - i).rjust(3), word) 35 | print string 36 | def printCallStack(self): 37 | self.printStack(self.call_stack) 38 | def printDataStack(self): 39 | self.printStack(self.data_stack) 40 | 41 | def spin(self): 42 | while not rospy.is_shutdown(): 43 | rospy.sleep(0.2) 44 | if self.stack_to_use == "call": 45 | self.printCallStack() 46 | elif self.stack_to_use == "data": 47 | self.printDataStack() 48 | else: 49 | raise ValueError("Bad stack: %s" % self.stack_to_use) 50 | def hangup(signal, stackframe): 51 | import signal 52 | import os 53 | os.kill(os.getpid(), signal.SIGTERM) 54 | 55 | def main(): 56 | import sys 57 | import signal 58 | if (len(sys.argv) != 3): 59 | print "usage: ein_stack.py left|right call|data" 60 | return 61 | 62 | arm = sys.argv[1] 63 | stack = sys.argv[2] 64 | 65 | if arm not in ("left", "right"): 66 | print "Arm must be left or right, not", arm 67 | return 68 | if stack not in ("call", "data"): 69 | print "Stack must be call or data, not", stack 70 | return 71 | 72 | rospy.init_node("ein_stack_%s" % arm, anonymous=True) 73 | 74 | signal.signal(signal.SIGHUP, hangup) 75 | client = EinStack("/ein/%s/state" % arm, stack) 76 | client.spin() 77 | 78 | 79 | 80 | if __name__=='__main__': 81 | main() 82 | -------------------------------------------------------------------------------- /python/ein_print_state.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import math 4 | from ein.msg import EinState 5 | import os 6 | class EinPrint: 7 | def __init__(self, topic): 8 | 9 | self.state_subscriber = rospy.Subscriber(topic, 10 | EinState, self.state_callback, queue_size=1) 11 | self.state = None 12 | self.call_stack = [] 13 | self.data_stack = [] 14 | self.lastrun = rospy.get_rostime() 15 | 16 | def state_callback(self, msg): 17 | if rospy.get_rostime() - self.lastrun < rospy.Duration(0.25): 18 | return 19 | 20 | self.lastrun = rospy.get_rostime() 21 | 22 | state = str(msg.state_string) 23 | 24 | rows, cols = os.popen('stty size', 'r').read().split() 25 | rows = int(rows) 26 | cols = int(cols) 27 | 28 | state_lines = state.split("\n") 29 | num_lines = sum([max(1, int(math.ceil(len(w) / float(cols)))) for w in state_lines]) 30 | #print "num_lines", num_lines 31 | lines_to_add = max(0, rows - num_lines - 1) 32 | state +="".rjust(lines_to_add, "\n") 33 | #print "num_lines: ", num_lines, "rows", rows, "lines to add", lines_to_add 34 | print state 35 | rospy.sleep(0.1) 36 | def spin(self): 37 | while not rospy.is_shutdown(): 38 | rospy.sleep(0.3) 39 | def hangup(signal, stackframe): 40 | import signal 41 | import os 42 | os.kill(os.getpid(), signal.SIGTERM) 43 | 44 | def main(): 45 | import sys 46 | import signal 47 | if (len(sys.argv) != 2): 48 | print "usage: ein_print_state.py left|right" 49 | return 50 | 51 | arm = sys.argv[1] 52 | signal.signal(signal.SIGHUP, hangup) 53 | 54 | rospy.init_node("ein_print_state_%s" % arm, anonymous=True) 55 | 56 | 57 | client = EinPrint("/ein/%s/state" % arm) 58 | client.spin() 59 | 60 | 61 | 62 | if __name__=='__main__': 63 | main() 64 | -------------------------------------------------------------------------------- /python/incomplete_beta_integral.py: -------------------------------------------------------------------------------- 1 | from scipy.special import betainc 2 | import matplotlib.pyplot as plt; plt.rc('text', usetex=True) 3 | import pylab as mpl 4 | import numpy as na 5 | 6 | def plot(successes, failures): 7 | a = successes + 1 8 | b = failures + 1 9 | 10 | X = na.arange(0, 1, 0.01) 11 | Y = [betainc(a, b, x) for x in X] 12 | mpl.plot(X, Y, label="successes=%d; failures=%d" % (successes, failures)) 13 | 14 | if (successes + failures == 0): 15 | mu = 0.5 16 | else: 17 | mu = float(successes) / (successes + failures) 18 | 19 | muX = [mu] 20 | muY = [0.5] 21 | mpl.scatter(muX, muY, marker="o", color="k") # plot mean given successes and failures 22 | mpl.ylabel("$\Pr(\mu <= x)$") 23 | mpl.xlabel("$x$") 24 | mpl.legend(loc="lower right") 25 | 26 | def main(): 27 | """ 28 | Smoke test for incomplete beta. 29 | """ 30 | plot(0, 0) 31 | plot(5, 5) 32 | plot(5, 10) 33 | plot(10, 5) 34 | mpl.title("Value of the incomplete beta function for different values of $x$") 35 | mpl.show() 36 | 37 | def printIncbet(): 38 | successes = 1 39 | failures = 0 40 | for d in na.arange(0, 1, 0.01): 41 | result = betainc(successes + 1, failures + 1, d); 42 | print "Result: S:", successes, 43 | print "F:", failures, "d:", d, "inc: %.5f" % result 44 | 45 | if __name__ == "__main__": 46 | printIncbet() 47 | #main() 48 | 49 | -------------------------------------------------------------------------------- /python/test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # license removed for brevity 3 | import rospy 4 | import time 5 | from std_msgs.msg import String 6 | import socket 7 | import sys 8 | 9 | HOST = '' 10 | PORT = 12346 11 | SIZE = 1024 12 | s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 13 | s.bind((HOST, PORT)) 14 | 15 | def talker(): 16 | pub = rospy.Publisher('/ein/right/forth_commands', String, queue_size=0) 17 | rospy.init_node('commands', anonymous=True) 18 | rate = rospy.Rate(10) # 10hz 19 | start_str = "\"aibo\" import" 20 | rospy.loginfo(start_str) 21 | pub.publish(start_str) 22 | time.sleep(3) 23 | summon_str = "dogSummonPluto" 24 | rospy.loginfo(summon_str) 25 | pub.publish(summon_str) 26 | time.sleep(3) 27 | while True: 28 | (data, addr) = s.recvfrom(SIZE) 29 | print(data) 30 | #sys.exit() 31 | if not rospy.is_shutdown(): 32 | execute_str = data 33 | rospy.loginfo(execute_str) 34 | pub.publish(execute_str) 35 | rate.sleep() 36 | time.sleep(2.0) 37 | 38 | if __name__ == '__main__': 39 | try: 40 | talker() 41 | except rospy.ROSInterruptException: 42 | pass 43 | 44 | -------------------------------------------------------------------------------- /python/warehouse_client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import time 4 | import std_msgs 5 | import roslib 6 | #roslib.load_manifest("ein") 7 | from object_recognition_msgs.msg import RecognizedObjectArray 8 | import baxter_external_devices 9 | 10 | class WarehouseClient: 11 | def __init__(self, givenTopic): 12 | self.command_publisher = rospy.Publisher("/fetch_commands", 13 | std_msgs.msg.String, queue_size=10) 14 | self.objects = [] 15 | #topic = "/publish_detections_center/blue_labeled_objects" # node 16 | #topic = "/ar_objects" # ar tags 17 | #topic = "/ein_right/blue_memory_objects" # ein 18 | #topic = "/ein_left/blue_memory_objects" # ein 19 | topic = givenTopic 20 | print topic 21 | rospy.Subscriber(topic, RecognizedObjectArray, self.object_callback) 22 | self.last_object_callback = rospy.Time() 23 | rospy.Timer(rospy.Duration(2), self.timer) 24 | 25 | def timer(self, event): 26 | if (self.last_object_callback - rospy.Time.now() > rospy.Duration(10)): 27 | self.objects = [] 28 | 29 | def object_callback(self, msg): 30 | self.objects = [] 31 | self.object_poses = dict() 32 | for o in msg.objects: 33 | self.objects.append(o.type.key) 34 | self.objects.sort() 35 | self.objects = tuple(self.objects) 36 | self.last_object_callback = rospy.Time() 37 | 38 | def ask(self): 39 | done = False 40 | printed_objects = None 41 | 42 | while not done and not rospy.is_shutdown(): 43 | if printed_objects != self.objects: 44 | printed_objects = self.objects 45 | print "Which object?" 46 | for i, o in enumerate(self.objects): 47 | print "%d.)" % (i + 1), o 48 | 49 | c = baxter_external_devices.getch() 50 | if c: 51 | if c in ['\x1b', '\x03']: 52 | done = True 53 | else: 54 | try: 55 | idx = int(c) 56 | except ValueError: 57 | idx = 0 58 | if idx != None and idx > 0 and idx <= len(self.objects): 59 | print "Sending", self.objects[idx - 1] 60 | self.command_publisher.publish(self.objects[idx - 1]) 61 | def main(): 62 | import sys 63 | if (len(sys.argv) != 2): 64 | print "usage: warehouse_client.py left|right" 65 | return 66 | arm = sys.argv[1] 67 | 68 | rospy.init_node("warehouse_client_%s" % arm) 69 | client = WarehouseClient("/ein_%s/blue_memory_objects" % arm) 70 | 71 | client.ask() 72 | 73 | 74 | 75 | if __name__=='__main__': 76 | main() 77 | -------------------------------------------------------------------------------- /scripts/sendToBothArms.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rostopic pub -1 /ein/right/forth_commands std_msgs/String "$1" & rostopic pub -1 /ein/left/forth_commands std_msgs/String "$1" 4 | -------------------------------------------------------------------------------- /setup.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # This file, when sourced, sets your ROS environment variables. It 4 | # should be placed in the root of the catkin workspace, since it also 5 | # sources the catkin setup.bash file. It takes a ROBOT= enviornment 6 | # variable which it uses to set things. You may need to edit this 7 | # file depending on your network configuration to set ROS_IP or 8 | # ROS_HOSTNAME for your machine. 9 | 10 | if [ -z ${ROBOT} ]; then 11 | export ROBOT=localhost 12 | fi 13 | 14 | source devel/setup.bash 15 | #export ROS_IP=192.168.42.1 16 | export ROS_HOSTNAME=`hostname` 17 | export ROS_MASTER_URI=http://$ROBOT:11311 18 | export PS1="\[\033[00;33m\][pidrone - ${ROS_MASTER_URI}]\[\033[00m\] $PS1" 19 | 20 | # If you are using baxter, make this file source baxter.sh and remove 21 | # the othe rstuff above, which is redundant if you are using 22 | # baxter.sh. 23 | 24 | # source baxter.sh 25 | -------------------------------------------------------------------------------- /src/aibo/ein_aibo.h: -------------------------------------------------------------------------------- 1 | #ifndef _EIN_AIBO_H_ 2 | #define _EIN_AIBO_H_ 3 | 4 | class EinAiboSensors; 5 | class EinAiboJoints; 6 | class EinAiboIndicators; 7 | class AiboPoseWord; 8 | class EinAiboConfig; 9 | class MachineState; 10 | 11 | void robotInitializeConfig(MachineState * ms); 12 | void robotInitializeMachine(MachineState * ms); 13 | void robotEndPointCallback(MachineState * ms); 14 | void robotSetCurrentJointPositions(MachineState * ms); 15 | 16 | void robotActivateSensorStreaming(MachineState * ms); 17 | void robotDeactivateSensorStreaming(MachineState * ms); 18 | void robotUpdate(MachineState * ms); 19 | 20 | #endif /* _EIN_AIBO_H_*/ 21 | -------------------------------------------------------------------------------- /src/aibo/ein_aibo_reactive.cpp: -------------------------------------------------------------------------------- 1 | #include "ein_words.h" 2 | #include "config.h" 3 | #include "ein.h" 4 | #include "camera.h" 5 | 6 | #include "ein_aibo.h" 7 | #include "ein_aibo_config.h" 8 | 9 | void EinAiboDog::robotUpdate() { 10 | 11 | } 12 | -------------------------------------------------------------------------------- /src/base64.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | std::string base64_encode(unsigned char const* , unsigned int len); 4 | std::string base64_decode(std::string const& s); 5 | -------------------------------------------------------------------------------- /src/baxter/ein_baxter.h: -------------------------------------------------------------------------------- 1 | #ifndef _EIN_BAXTER_H_ 2 | #define _EIN_BAXTER_H_ 3 | 4 | class EinBaxterConfig; 5 | class MachineState; 6 | void robotInitializeSerial(MachineState * ms); 7 | void robotInitializeConfig(MachineState * ms); 8 | void robotInitializeMachine(MachineState * ms); 9 | void robotEndPointCallback(MachineState * ms); 10 | void robotSetCurrentJointPositions(MachineState * ms); 11 | 12 | void robotActivateSensorStreaming(MachineState * ms); 13 | void robotDeactivateSensorStreaming(MachineState * ms); 14 | void robotUpdate(MachineState * ms); 15 | #include 16 | #include 17 | #endif /* _EIN_BAXTER_H_ */ 18 | -------------------------------------------------------------------------------- /src/baxter/ein_ik.h: -------------------------------------------------------------------------------- 1 | #ifndef _EIN_IK_H_ 2 | #define _EIN_IK_H_ 3 | 4 | #include "ein_baxter_config.h" 5 | 6 | ikMapState ikAtPose(MachineState * ms, eePose pose); 7 | vector ikAtPoses(MachineState * ms, vector poses); 8 | 9 | bool willIkResultFail(MachineState * ms, baxter_core_msgs::SolvePositionIK thisIkRequest, int thisIkCallResult, bool * likelyInCollision, int i); 10 | 11 | void fillIkRequest(eePose givenEEPose, baxter_core_msgs::SolvePositionIK * givenIkRequest); 12 | void fillIkRequest(vector poses, baxter_core_msgs::SolvePositionIK * givenIkRequest); 13 | 14 | void reseedIkRequest(MachineState * ms, eePose *givenEEPose, baxter_core_msgs::SolvePositionIK * givenIkRequest, int it, int itMax); 15 | 16 | void queryIK(MachineState * ms, int * thisResult, baxter_core_msgs::SolvePositionIK * thisRequest); 17 | 18 | #endif /* _EIN_IK_H_ */ 19 | -------------------------------------------------------------------------------- /src/baxter/ikfast/ikfast_wrapper.h: -------------------------------------------------------------------------------- 1 | #include "ein.h" 2 | #include "baxter/ein_baxter_config.h" 3 | #include 4 | 5 | 6 | 7 | namespace MY_NAMESPACE { 8 | 9 | void queryIKFast(MachineState * ms, int * thisResult, baxter_core_msgs::SolvePositionIK * thisRequest); 10 | void queryIKFastDebug(MachineState * ms, int * thisResult, baxter_core_msgs::SolvePositionIK * thisRequest); 11 | 12 | eePose ikfast_computeFK(MachineState * ms, vector joint_angles); 13 | 14 | } 15 | -------------------------------------------------------------------------------- /src/baxter/ikfast/ikfast_wrapper_left.cpp: -------------------------------------------------------------------------------- 1 | #include "ikfast_wrapper_left.h" 2 | 3 | #include "ikfast_wrapper.cpp" 4 | -------------------------------------------------------------------------------- /src/baxter/ikfast/ikfast_wrapper_left.h: -------------------------------------------------------------------------------- 1 | 2 | #define IKFAST_NAMESPACE ikfast_left 3 | #define IKFAST_SOLVER_CPP "baxter/ikfast/baxter_left_arm_ikfast_solver.cpp" 4 | #define MY_NAMESPACE ikfast_left_ein 5 | 6 | #include "ikfast_wrapper.h" 7 | 8 | -------------------------------------------------------------------------------- /src/baxter/ikfast/ikfast_wrapper_right.cpp: -------------------------------------------------------------------------------- 1 | #include "ikfast_wrapper_right.h" 2 | 3 | #include "ikfast_wrapper.cpp" 4 | -------------------------------------------------------------------------------- /src/baxter/ikfast/ikfast_wrapper_right.h: -------------------------------------------------------------------------------- 1 | 2 | #define IKFAST_NAMESPACE ikfast_right 3 | #define IKFAST_SOLVER_CPP "baxter/ikfast/baxter_right_arm_ikfast_solver.cpp" 4 | #define MY_NAMESPACE ikfast_right_ein 5 | 6 | #include "ikfast_wrapper.h" 7 | 8 | -------------------------------------------------------------------------------- /src/cephes/Makefile: -------------------------------------------------------------------------------- 1 | LIB = cephes 2 | ROOTDIR=/usr/local 3 | BUILDIR=$(ROOTDIR) 4 | CCC=gcc 5 | CC = $(CCC) 6 | OPT=-O2 -march=pentiumpro 7 | CFLAGS=$(OPT) 8 | 9 | OBJS = airy.o bdtr.o beta.o \ 10 | btdtr.o cbrt.o chbevl.o chdtr.o const.o \ 11 | dawsn.o ellie.o ellik.o ellpe.o ellpj.o ellpk.o \ 12 | exp10.o exp2.o expn.o fabs.o fdtr.o \ 13 | fresnl.o gamma.o gdtr.o hyp2f1.o hyperg.o i0.o i1.o igami.o \ 14 | incbet.o incbi.o igam.o iv.o j0.o j1.o jn.o jv.o k0.o k1.o \ 15 | kn.o kolmogorov.o nbdtr.o ndtr.o ndtri.o pdtr.o \ 16 | polevl.o polmisc.o polyn.o psi.o rgamma.o round.o \ 17 | shichi.o sici.o sindg.o spence.o stdtr.o struve.o \ 18 | tandg.o unity.o yn.o zeta.o zetac.o \ 19 | setprec.o mtherr.o 20 | 21 | 22 | lib$(LIB).a: $(OBJS) 23 | ar ru lib$(LIB).a $? 24 | ranlib lib$(LIB).a 25 | 26 | install: lib$(LIB).a 27 | install -c lib$(LIB).a $(BUILDIR)/lib 28 | 29 | clean: 30 | rm -f *.o *.so *.lo 31 | 32 | -------------------------------------------------------------------------------- /src/cephes/btdtr.c: -------------------------------------------------------------------------------- 1 | 2 | /* btdtr.c 3 | * 4 | * Beta distribution 5 | * 6 | * 7 | * 8 | * SYNOPSIS: 9 | * 10 | * double a, b, x, y, btdtr(); 11 | * 12 | * y = btdtr( a, b, x ); 13 | * 14 | * 15 | * 16 | * DESCRIPTION: 17 | * 18 | * Returns the area from zero to x under the beta density 19 | * function: 20 | * 21 | * 22 | * x 23 | * - - 24 | * | (a+b) | | a-1 b-1 25 | * P(x) = ---------- | t (1-t) dt 26 | * - - | | 27 | * | (a) | (b) - 28 | * 0 29 | * 30 | * 31 | * This function is identical to the incomplete beta 32 | * integral function incbet(a, b, x). 33 | * 34 | * The complemented function is 35 | * 36 | * 1 - P(1-x) = incbet( b, a, x ); 37 | * 38 | * 39 | * ACCURACY: 40 | * 41 | * See incbet.c. 42 | * 43 | */ 44 | 45 | /* btdtr() */ 46 | 47 | 48 | /* 49 | * Cephes Math Library Release 2.0: April, 1987 50 | * Copyright 1984, 1987, 1995 by Stephen L. Moshier 51 | */ 52 | 53 | #include "mconf.h" 54 | 55 | double btdtr(a, b, x) 56 | double a, b, x; 57 | { 58 | 59 | return (incbet(a, b, x)); 60 | } 61 | -------------------------------------------------------------------------------- /src/cephes/cephes_names.h: -------------------------------------------------------------------------------- 1 | #ifndef CEPHES_NAMES_H 2 | #define CEPHES_NAMES_H 3 | 4 | #define airy cephes_airy 5 | #define bdtrc cephes_bdtrc 6 | #define bdtr cephes_bdtr 7 | #define bdtri cephes_bdtri 8 | #define beta cephes_beta 9 | #define lbeta cephes_lbeta 10 | #define btdtr cephes_btdtr 11 | #define cbrt cephes_cbrt 12 | #define chdtrc cephes_chdtrc 13 | #define chdtr cephes_chdtr 14 | #define chdtri cephes_chdtri 15 | #define dawsn cephes_dawsn 16 | #define ellie cephes_ellie 17 | #define ellik cephes_ellik 18 | #define ellpe cephes_ellpe 19 | #define ellpj cephes_ellpj 20 | #define ellpk cephes_ellpk 21 | #define exp10 cephes_exp10 22 | #define exp1m cephes_exp1m 23 | #define exp2 cephes_exp2 24 | #define expn cephes_expn 25 | #define fdtrc cephes_fdtrc 26 | #define fdtr cephes_fdtr 27 | #define fdtri cephes_fdtri 28 | #define fresnl cephes_fresnl 29 | #define Gamma cephes_Gamma 30 | #define lgam cephes_lgam 31 | #define gdtr cephes_gdtr 32 | #define gdtrc cephes_gdtrc 33 | #define gdtri cephes_gdtri 34 | #define hyp2f1 cephes_hyp2f1 35 | #define hyperg cephes_hyperg 36 | #define hyp2f0 cephes_hyp2f0 37 | #define onef2 cephes_onef2 38 | #define threef0 cephes_threef0 39 | #define i0 cephes_i0 40 | #define i0e cephes_i0e 41 | #define i1 cephes_i1 42 | #define i1e cephes_i1e 43 | #define igamc cephes_igamc 44 | #define igam cephes_igam 45 | #define igami cephes_igami 46 | #define incbet cephes_incbet 47 | #define incbi cephes_incbi 48 | #define iv cephes_iv 49 | #define j0 cephes_j0 50 | #define y0 cephes_y0 51 | #define j1 cephes_j1 52 | #define y1 cephes_y1 53 | #define jn cephes_jn 54 | #define jv cephes_jv 55 | #define k0 cephes_k0 56 | #define k0e cephes_k0e 57 | #define k1 cephes_k1 58 | #define k1e cephes_k1e 59 | #define kn cephes_kn 60 | #define nbdtrc cephes_nbdtrc 61 | #define nbdtr cephes_nbdtr 62 | #define nbdtri cephes_nbdtri 63 | #define ndtr cephes_ndtr 64 | #define erfc cephes_erfc 65 | #define erf cephes_erf 66 | #define ndtri cephes_ndtri 67 | #define pdtrc cephes_pdtrc 68 | #define pdtr cephes_pdtr 69 | #define pdtri cephes_pdtri 70 | #define psi cephes_psi 71 | #define rgamma cephes_rgamma 72 | #define round cephes_round 73 | #define shichi cephes_shichi 74 | #define sici cephes_sici 75 | #define radian cephes_radian 76 | #define sindg cephes_sindg 77 | #define cosdg cephes_cosdg 78 | #define sincos cephes_sincos 79 | #define spence cephes_spence 80 | #define stdtr cephes_stdtr 81 | #define stdtri cephes_stdtri 82 | #define struve cephes_struve 83 | #define yv cephes_yv 84 | #define tandg cephes_tandg 85 | #define cotdg cephes_cotdg 86 | #define log1p cephes_log1p 87 | #define expm1 cephes_expm1 88 | #define cosm1 cephes_cosm1 89 | #define yn cephes_yn 90 | #define zeta cephes_zeta 91 | #define zetac cephes_zetac 92 | #define smirnov cephes_smirnov 93 | #define smirnovi cephes_smirnovi 94 | #define kolmogorov cephes_kolmogorov 95 | #define kolmogi cephes_kolmogi 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /src/cephes/chbevl.c: -------------------------------------------------------------------------------- 1 | /* chbevl.c 2 | * 3 | * Evaluate Chebyshev series 4 | * 5 | * 6 | * 7 | * SYNOPSIS: 8 | * 9 | * int N; 10 | * double x, y, coef[N], chebevl(); 11 | * 12 | * y = chbevl( x, coef, N ); 13 | * 14 | * 15 | * 16 | * DESCRIPTION: 17 | * 18 | * Evaluates the series 19 | * 20 | * N-1 21 | * - ' 22 | * y = > coef[i] T (x/2) 23 | * - i 24 | * i=0 25 | * 26 | * of Chebyshev polynomials Ti at argument x/2. 27 | * 28 | * Coefficients are stored in reverse order, i.e. the zero 29 | * order term is last in the array. Note N is the number of 30 | * coefficients, not the order. 31 | * 32 | * If coefficients are for the interval a to b, x must 33 | * have been transformed to x -> 2(2x - b - a)/(b-a) before 34 | * entering the routine. This maps x from (a, b) to (-1, 1), 35 | * over which the Chebyshev polynomials are defined. 36 | * 37 | * If the coefficients are for the inverted interval, in 38 | * which (a, b) is mapped to (1/b, 1/a), the transformation 39 | * required is x -> 2(2ab/x - b - a)/(b-a). If b is infinity, 40 | * this becomes x -> 4a/x - 1. 41 | * 42 | * 43 | * 44 | * SPEED: 45 | * 46 | * Taking advantage of the recurrence properties of the 47 | * Chebyshev polynomials, the routine requires one more 48 | * addition per loop than evaluating a nested polynomial of 49 | * the same degree. 50 | * 51 | */ 52 | /* chbevl.c */ 53 | 54 | /* 55 | * Cephes Math Library Release 2.0: April, 1987 56 | * Copyright 1985, 1987 by Stephen L. Moshier 57 | * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 58 | */ 59 | 60 | #include 61 | #include "protos.h" 62 | 63 | double chbevl(x, array, n) 64 | double x; 65 | double array[]; 66 | int n; 67 | { 68 | double b0, b1, b2, *p; 69 | int i; 70 | 71 | p = array; 72 | b0 = *p++; 73 | b1 = 0.0; 74 | i = n - 1; 75 | 76 | do { 77 | b2 = b1; 78 | b1 = b0; 79 | b0 = x * b1 - b2 + *p++; 80 | } 81 | while (--i); 82 | 83 | return (0.5 * (b0 - b2)); 84 | } 85 | -------------------------------------------------------------------------------- /src/cephes/gdtr.c: -------------------------------------------------------------------------------- 1 | /* gdtr.c 2 | * 3 | * Gamma distribution function 4 | * 5 | * 6 | * 7 | * SYNOPSIS: 8 | * 9 | * double a, b, x, y, gdtr(); 10 | * 11 | * y = gdtr( a, b, x ); 12 | * 13 | * 14 | * 15 | * DESCRIPTION: 16 | * 17 | * Returns the integral from zero to x of the Gamma probability 18 | * density function: 19 | * 20 | * 21 | * x 22 | * b - 23 | * a | | b-1 -at 24 | * y = ----- | t e dt 25 | * - | | 26 | * | (b) - 27 | * 0 28 | * 29 | * The incomplete Gamma integral is used, according to the 30 | * relation 31 | * 32 | * y = igam( b, ax ). 33 | * 34 | * 35 | * ACCURACY: 36 | * 37 | * See igam(). 38 | * 39 | * ERROR MESSAGES: 40 | * 41 | * message condition value returned 42 | * gdtr domain x < 0 0.0 43 | * 44 | */ 45 | /* gdtrc.c 46 | * 47 | * Complemented Gamma distribution function 48 | * 49 | * 50 | * 51 | * SYNOPSIS: 52 | * 53 | * double a, b, x, y, gdtrc(); 54 | * 55 | * y = gdtrc( a, b, x ); 56 | * 57 | * 58 | * 59 | * DESCRIPTION: 60 | * 61 | * Returns the integral from x to infinity of the Gamma 62 | * probability density function: 63 | * 64 | * 65 | * inf. 66 | * b - 67 | * a | | b-1 -at 68 | * y = ----- | t e dt 69 | * - | | 70 | * | (b) - 71 | * x 72 | * 73 | * The incomplete Gamma integral is used, according to the 74 | * relation 75 | * 76 | * y = igamc( b, ax ). 77 | * 78 | * 79 | * ACCURACY: 80 | * 81 | * See igamc(). 82 | * 83 | * ERROR MESSAGES: 84 | * 85 | * message condition value returned 86 | * gdtrc domain x < 0 0.0 87 | * 88 | */ 89 | 90 | /* gdtr() */ 91 | 92 | 93 | /* 94 | * Cephes Math Library Release 2.3: March,1995 95 | * Copyright 1984, 1987, 1995 by Stephen L. Moshier 96 | */ 97 | 98 | #include "mconf.h" 99 | double gdtri(double, double, double); 100 | 101 | double gdtr(a, b, x) 102 | double a, b, x; 103 | { 104 | 105 | if (x < 0.0) { 106 | mtherr("gdtr", DOMAIN); 107 | return (NPY_NAN); 108 | } 109 | return (igam(b, a * x)); 110 | } 111 | 112 | 113 | double gdtrc(a, b, x) 114 | double a, b, x; 115 | { 116 | 117 | if (x < 0.0) { 118 | mtherr("gdtrc", DOMAIN); 119 | return (NPY_NAN); 120 | } 121 | return (igamc(b, a * x)); 122 | } 123 | 124 | 125 | double gdtri(a, b, y) 126 | double a, b, y; 127 | { 128 | 129 | if ((y < 0.0) || (y > 1.0) || (a <= 0.0) || (b < 0.0)) { 130 | mtherr("gdtri", DOMAIN); 131 | return (NPY_NAN); 132 | } 133 | 134 | return (igami(b, 1.0 - y) / a); 135 | } 136 | -------------------------------------------------------------------------------- /src/cephes/mmmpy.c: -------------------------------------------------------------------------------- 1 | /* mmmpy.c 2 | * 3 | * Matrix multiply 4 | * 5 | * 6 | * 7 | * SYNOPSIS: 8 | * 9 | * int r, c; 10 | * double A[r*c], B[c*r], Y[r*r]; 11 | * 12 | * mmmpy( r, c, A, B, Y ); 13 | * 14 | * 15 | * 16 | * DESCRIPTION: 17 | * 18 | * Y = A B 19 | * c-1 20 | * -- 21 | * Y[i][j] = > A[i][k] B[k][j] 22 | * -- 23 | * k=0 24 | * 25 | * Multiplies an r (rows) by c (columns) matrix A on the left 26 | * by a c (rows) by r (columns) matrix B on the right 27 | * to produce an r by r matrix Y. 28 | * 29 | * 30 | */ 31 | 32 | #include "protos.h" 33 | 34 | void mmmpy(r, c, A, B, Y) 35 | int r, c; 36 | double *A, *B, *Y; 37 | { 38 | register double s; 39 | double *pA, *pB, *pY, *pt; 40 | int i, j, k; 41 | 42 | pY = Y; 43 | pB = B; 44 | for (i = 0; i < r; i++) { 45 | pA = A; 46 | for (j = 0; j < r; j++) { 47 | pt = pB; 48 | s = 0.0; 49 | for (k = 0; k < c; k++) { 50 | s += *pA++ * *pt; 51 | pt += r; /* increment to next row underneath */ 52 | } 53 | *pY++ = s; 54 | } 55 | pB += 1; 56 | } 57 | } 58 | -------------------------------------------------------------------------------- /src/cephes/mtherr.c: -------------------------------------------------------------------------------- 1 | /* mtherr.c 2 | * 3 | * Library common error handling routine 4 | * 5 | * 6 | * 7 | * SYNOPSIS: 8 | * 9 | * char *fctnam; 10 | * int code; 11 | * int mtherr(); 12 | * 13 | * mtherr( fctnam, code ); 14 | * 15 | * 16 | * 17 | * DESCRIPTION: 18 | * 19 | * This routine may be called to report one of the following 20 | * error conditions (in the include file mconf.h). 21 | * 22 | * Mnemonic Value Significance 23 | * 24 | * DOMAIN 1 argument domain error 25 | * SING 2 function singularity 26 | * OVERFLOW 3 overflow range error 27 | * UNDERFLOW 4 underflow range error 28 | * TLOSS 5 total loss of precision 29 | * PLOSS 6 partial loss of precision 30 | * EDOM 33 Unix domain error code 31 | * ERANGE 34 Unix range error code 32 | * 33 | * The default version of the file prints the function name, 34 | * passed to it by the pointer fctnam, followed by the 35 | * error condition. The display is directed to the standard 36 | * output device. The routine then returns to the calling 37 | * program. Users may wish to modify the program to abort by 38 | * calling exit() under severe error conditions such as domain 39 | * errors. 40 | * 41 | * Since all error conditions pass control to this function, 42 | * the display may be easily changed, eliminated, or directed 43 | * to an error logging device. 44 | * 45 | * SEE ALSO: 46 | * 47 | * mconf.h 48 | * 49 | */ 50 | 51 | /* 52 | * Cephes Math Library Release 2.0: April, 1987 53 | * Copyright 1984, 1987 by Stephen L. Moshier 54 | * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 55 | */ 56 | 57 | #include 58 | #include "mconf.h" 59 | 60 | int merror = 0; 61 | 62 | 63 | int mtherr(char *name, int code) 64 | { 65 | /* Display string passed by calling program, 66 | * which is supposed to be the name of the 67 | * function in which the error occurred: 68 | */ 69 | 70 | /* Set global error message word */ 71 | merror = code; 72 | 73 | /* Display error message defined 74 | * by the code argument. 75 | */ 76 | if ((code <= 0) || (code >= 8)) 77 | code = 0; 78 | 79 | printf("cephes error: %s code %d", name, code); 80 | 81 | /* Return to calling 82 | * program 83 | */ 84 | return (0); 85 | } 86 | -------------------------------------------------------------------------------- /src/cephes/mtransp.c: -------------------------------------------------------------------------------- 1 | /* mtransp.c 2 | * 3 | * Matrix transpose 4 | * 5 | * 6 | * 7 | * SYNOPSIS: 8 | * 9 | * int n; 10 | * double A[n*n], T[n*n]; 11 | * 12 | * mtransp( n, A, T ); 13 | * 14 | * 15 | * 16 | * DESCRIPTION: 17 | * 18 | * 19 | * T[r][c] = A[c][r] 20 | * 21 | * 22 | * Transposes the n by n square matrix A and puts the result in T. 23 | * The output, T, may occupy the same storage as A. 24 | * 25 | * 26 | * 27 | */ 28 | 29 | 30 | void mtransp(int, double *, double *); 31 | 32 | void mtransp(n, A, T) 33 | int n; 34 | double *A, *T; 35 | { 36 | int i, j, np1; 37 | double *pAc, *pAr, *pTc, *pTr, *pA0, *pT0; 38 | double x; 39 | 40 | np1 = n + 1; 41 | pA0 = A; 42 | pT0 = T; 43 | for (i = 0; i < n - 1; i++) { /* row index */ 44 | pAc = pA0; /* next diagonal element of input */ 45 | pAr = pAc + n; /* next row down underneath the diagonal element */ 46 | pTc = pT0; /* next diagonal element of the output */ 47 | pTr = pTc + n; /* next row underneath */ 48 | *pTc++ = *pAc++; /* copy the diagonal element */ 49 | for (j = i + 1; j < n; j++) { /* column index */ 50 | x = *pAr; 51 | *pTr = *pAc++; 52 | *pTc++ = x; 53 | pAr += n; 54 | pTr += n; 55 | } 56 | pA0 += np1; /* &A[n*i+i] for next i */ 57 | pT0 += np1; /* &T[n*i+i] for next i */ 58 | } 59 | *pT0 = *pA0; /* copy the diagonal element */ 60 | } 61 | -------------------------------------------------------------------------------- /src/cephes/mvmpy.c: -------------------------------------------------------------------------------- 1 | /* mvmpy.c 2 | * 3 | * Matrix times vector 4 | * 5 | * 6 | * 7 | * SYNOPSIS: 8 | * 9 | * int r, c; 10 | * double A[r*c], V[c], Y[r]; 11 | * 12 | * mvmpy( r, c, A, V, Y ); 13 | * 14 | * 15 | * 16 | * DESCRIPTION: 17 | * 18 | * c-1 19 | * -- 20 | * Y[j] = > A[j][k] V[k] , j = 1, ..., r 21 | * -- 22 | * k=0 23 | * 24 | * Multiplies the r (rows) by c (columns) matrix A on the left 25 | * by column vector V of dimension c on the right 26 | * to produce a (column) vector Y output of dimension r. 27 | * 28 | * 29 | * 30 | * 31 | */ 32 | 33 | 34 | void mvmpy(int r, int c, double *A, double *V, double *Y) 35 | { 36 | register double s; 37 | double *pA, *pV, *pY; 38 | int i, j; 39 | 40 | pA = A; 41 | pY = Y; 42 | for (i = 0; i < r; i++) { 43 | pV = V; 44 | s = 0.0; 45 | for (j = 0; j < c; j++) { 46 | s += *pA++ * *pV++; 47 | } 48 | *pY++ = s; 49 | } 50 | } 51 | -------------------------------------------------------------------------------- /src/cephes/polevl.c: -------------------------------------------------------------------------------- 1 | /* polevl.c 2 | * p1evl.c 3 | * 4 | * Evaluate polynomial 5 | * 6 | * 7 | * 8 | * SYNOPSIS: 9 | * 10 | * int N; 11 | * double x, y, coef[N+1], polevl[]; 12 | * 13 | * y = polevl( x, coef, N ); 14 | * 15 | * 16 | * 17 | * DESCRIPTION: 18 | * 19 | * Evaluates polynomial of degree N: 20 | * 21 | * 2 N 22 | * y = C + C x + C x +...+ C x 23 | * 0 1 2 N 24 | * 25 | * Coefficients are stored in reverse order: 26 | * 27 | * coef[0] = C , ..., coef[N] = C . 28 | * N 0 29 | * 30 | * The function p1evl() assumes that coef[N] = 1.0 and is 31 | * omitted from the array. Its calling arguments are 32 | * otherwise the same as polevl(). 33 | * 34 | * 35 | * SPEED: 36 | * 37 | * In the interest of speed, there are no checks for out 38 | * of bounds arithmetic. This routine is used by most of 39 | * the functions in the library. Depending on available 40 | * equipment features, the user may wish to rewrite the 41 | * program in microcode or assembly language. 42 | * 43 | */ 44 | 45 | 46 | /* 47 | * Cephes Math Library Release 2.1: December, 1988 48 | * Copyright 1984, 1987, 1988 by Stephen L. Moshier 49 | * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 50 | */ 51 | #include "protos.h" 52 | 53 | double polevl(x, coef, N) 54 | double x; 55 | double coef[]; 56 | int N; 57 | { 58 | double ans; 59 | int i; 60 | double *p; 61 | 62 | p = coef; 63 | ans = *p++; 64 | i = N; 65 | 66 | do 67 | ans = ans * x + *p++; 68 | while (--i); 69 | 70 | return (ans); 71 | } 72 | 73 | /* p1evl() */ 74 | /* N 75 | * Evaluate polynomial when coefficient of x is 1.0. 76 | * Otherwise same as polevl. 77 | */ 78 | 79 | double p1evl(x, coef, N) 80 | double x; 81 | double coef[]; 82 | int N; 83 | { 84 | double ans; 85 | double *p; 86 | int i; 87 | 88 | p = coef; 89 | ans = x + *p++; 90 | i = N - 1; 91 | 92 | do 93 | ans = ans * x + *p++; 94 | while (--i); 95 | 96 | return (ans); 97 | } 98 | -------------------------------------------------------------------------------- /src/cephes/round.c: -------------------------------------------------------------------------------- 1 | /* round.c 2 | * 3 | * Round double to nearest or even integer valued double 4 | * 5 | * 6 | * 7 | * SYNOPSIS: 8 | * 9 | * double x, y, round(); 10 | * 11 | * y = round(x); 12 | * 13 | * 14 | * 15 | * DESCRIPTION: 16 | * 17 | * Returns the nearest integer to x as a double precision 18 | * floating point result. If x ends in 0.5 exactly, the 19 | * nearest even integer is chosen. 20 | * 21 | * 22 | * 23 | * ACCURACY: 24 | * 25 | * If x is greater than 1/(2*MACHEP), its closest machine 26 | * representation is already an integer, so rounding does 27 | * not change it. 28 | */ 29 | 30 | /* 31 | * Cephes Math Library Release 2.1: January, 1989 32 | * Copyright 1984, 1987, 1989 by Stephen L. Moshier 33 | * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 34 | */ 35 | 36 | #include "mconf.h" 37 | 38 | double round(double x) 39 | { 40 | double y, r; 41 | 42 | /* Largest integer <= x */ 43 | y = floor(x); 44 | 45 | /* Fractional part */ 46 | r = x - y; 47 | 48 | /* Round up to nearest. */ 49 | if (r > 0.5) 50 | goto rndup; 51 | 52 | /* Round to even */ 53 | if (r == 0.5) { 54 | r = y - 2.0 * floor(0.5 * y); 55 | if (r == 1.0) { 56 | rndup: 57 | y += 1.0; 58 | } 59 | } 60 | 61 | /* Else round down. */ 62 | return (y); 63 | } 64 | -------------------------------------------------------------------------------- /src/cephes/setprec.c: -------------------------------------------------------------------------------- 1 | /* Null stubs for coprocessor precision settings */ 2 | 3 | int sprec(void) 4 | { 5 | return 0; 6 | } 7 | 8 | int dprec(void) 9 | { 10 | return 0; 11 | } 12 | 13 | int ldprec(void) 14 | { 15 | return 0; 16 | } 17 | -------------------------------------------------------------------------------- /src/cephes/simpsn.c: -------------------------------------------------------------------------------- 1 | /* simpsn.c */ 2 | /* simpsn.c 3 | * Numerical integration of function tabulated 4 | * at equally spaced arguments 5 | */ 6 | 7 | /* Coefficients for Cote integration formulas */ 8 | 9 | /* Note: these numbers were computed using 40-decimal precision. */ 10 | 11 | #define NCOTE 8 12 | 13 | /* 6th order formula */ 14 | /* 15 | * static double simcon[] = 16 | * { 17 | * 4.88095238095238095E-2, 18 | * 2.57142857142857142857E-1, 19 | * 3.2142857142857142857E-2, 20 | * 3.2380952380952380952E-1, 21 | * }; 22 | */ 23 | 24 | /* 8th order formula */ 25 | static double simcon[] = { 26 | 3.488536155202821869E-2, 27 | 2.076895943562610229E-1, 28 | -3.27336860670194003527E-2, 29 | 3.7022927689594356261E-1, 30 | -1.6014109347442680776E-1, 31 | }; 32 | 33 | /* 10th order formula */ 34 | /* 35 | * static double simcon[] = 36 | * { 37 | * 2.68341483619261397039E-2, 38 | * 1.77535941424830313719E-1, 39 | * -8.1043570626903960237E-2, 40 | * 4.5494628827962161295E-1, 41 | * -4.3515512265512265512E-1, 42 | * 7.1376463043129709796E-1, 43 | * }; 44 | */ 45 | 46 | /* simpsn.c 2 */ 47 | /* 20th order formula */ 48 | /* 49 | * static double simcon[] = 50 | * { 51 | * 1.182527324903160319E-2, 52 | * 1.14137717644606974987E-1, 53 | * -2.36478370511426964E-1, 54 | * 1.20618689348187566E+0, 55 | * -3.7710317267153304677E+0, 56 | * 1.03367982199398011435E+1, 57 | * -2.270881584397951229796E+1, 58 | * 4.1828057422193554603E+1, 59 | * -6.4075279490154004651555E+1, 60 | * 8.279728347247285172085E+1, 61 | * -9.0005367135242894657916E+1, 62 | * }; 63 | */ 64 | 65 | /* simpsn.c 3 */ 66 | 67 | double simpsn(double[], double); 68 | 69 | double simpsn(f, delta) 70 | double f[]; /* tabulated function */ 71 | double delta; /* spacing of arguments */ 72 | { 73 | extern double simcon[]; 74 | double ans; 75 | int i; 76 | 77 | 78 | ans = simcon[NCOTE / 2] * f[NCOTE / 2]; 79 | for (i = 0; i < NCOTE / 2; i++) 80 | ans += simcon[i] * (f[i] + f[NCOTE - i]); 81 | 82 | return (ans * delta * NCOTE); 83 | } 84 | -------------------------------------------------------------------------------- /src/cephes/tukey.c: -------------------------------------------------------------------------------- 1 | 2 | /* Compute the CDF of the Tukey-Lambda distribution 3 | * using a braketing search with special checks 4 | * 5 | * The PPF of the Tukey-lambda distribution is 6 | * G(p) = p**lam + (1-p)**lam / lam 7 | * 8 | * Author: Travis Oliphant 9 | */ 10 | 11 | #include 12 | 13 | #define SMALLVAL 1e-4 14 | #define EPS 1.0e-14 15 | #define MAXCOUNT 60 16 | 17 | double tukeylambdacdf(double x, double lmbda) 18 | { 19 | double pmin, pmid, pmax, plow, phigh, xeval; 20 | int count; 21 | 22 | xeval = 1.0 / lmbda; 23 | if (lmbda > 0.0) { 24 | if (x < (-xeval)) 25 | return 0.0; 26 | if (x > xeval) 27 | return 1.0; 28 | } 29 | 30 | if ((-SMALLVAL < lmbda) && (lmbda < SMALLVAL)) { 31 | if (x >= 0) 32 | return 1.0 / (1.0 + exp(-x)); 33 | else 34 | return exp(x) / (1.0 + exp(x)); 35 | } 36 | 37 | pmin = 0.0; 38 | pmid = 0.5; 39 | pmax = 1.0; 40 | plow = pmin; 41 | phigh = pmax; 42 | count = 0; 43 | 44 | while ((count < MAXCOUNT) && (fabs(pmid - plow) > EPS)) { 45 | xeval = (pow(pmid, lmbda) - pow(1.0 - pmid, lmbda)) / lmbda; 46 | if (xeval == x) 47 | return pmid; 48 | if (xeval > x) { 49 | phigh = pmid; 50 | pmid = (pmid + plow) / 2.0; 51 | } 52 | else { 53 | plow = pmid; 54 | pmid = (pmid + phigh) / 2.0; 55 | } 56 | count++; 57 | } 58 | return pmid; 59 | } 60 | -------------------------------------------------------------------------------- /src/cephes/yn.c: -------------------------------------------------------------------------------- 1 | /* yn.c 2 | * 3 | * Bessel function of second kind of integer order 4 | * 5 | * 6 | * 7 | * SYNOPSIS: 8 | * 9 | * double x, y, yn(); 10 | * int n; 11 | * 12 | * y = yn( n, x ); 13 | * 14 | * 15 | * 16 | * DESCRIPTION: 17 | * 18 | * Returns Bessel function of order n, where n is a 19 | * (possibly negative) integer. 20 | * 21 | * The function is evaluated by forward recurrence on 22 | * n, starting with values computed by the routines 23 | * y0() and y1(). 24 | * 25 | * If n = 0 or 1 the routine for y0 or y1 is called 26 | * directly. 27 | * 28 | * 29 | * 30 | * ACCURACY: 31 | * 32 | * 33 | * Absolute error, except relative 34 | * when y > 1: 35 | * arithmetic domain # trials peak rms 36 | * DEC 0, 30 2200 2.9e-16 5.3e-17 37 | * IEEE 0, 30 30000 3.4e-15 4.3e-16 38 | * 39 | * 40 | * ERROR MESSAGES: 41 | * 42 | * message condition value returned 43 | * yn singularity x = 0 NPY_INFINITY 44 | * yn overflow NPY_INFINITY 45 | * 46 | * Spot checked against tables for x, n between 0 and 100. 47 | * 48 | */ 49 | 50 | /* 51 | * Cephes Math Library Release 2.8: June, 2000 52 | * Copyright 1984, 1987, 2000 by Stephen L. Moshier 53 | */ 54 | 55 | #include "mconf.h" 56 | extern double MAXLOG; 57 | 58 | double yn(n, x) 59 | int n; 60 | double x; 61 | { 62 | double an, anm1, anm2, r; 63 | int k, sign; 64 | 65 | if (n < 0) { 66 | n = -n; 67 | if ((n & 1) == 0) /* -1**n */ 68 | sign = 1; 69 | else 70 | sign = -1; 71 | } 72 | else 73 | sign = 1; 74 | 75 | 76 | if (n == 0) 77 | return (sign * y0(x)); 78 | if (n == 1) 79 | return (sign * y1(x)); 80 | 81 | /* test for overflow */ 82 | if (x == 0.0) { 83 | mtherr("yn", SING); 84 | return -NPY_INFINITY * sign; 85 | } 86 | else if (x < 0.0) { 87 | mtherr("yn", DOMAIN); 88 | return NPY_NAN; 89 | } 90 | 91 | /* forward recurrence on n */ 92 | 93 | anm2 = y0(x); 94 | anm1 = y1(x); 95 | k = 1; 96 | r = 2 * k; 97 | do { 98 | an = r * anm1 / x - anm2; 99 | anm2 = anm1; 100 | anm1 = an; 101 | r += 2.0; 102 | ++k; 103 | } 104 | while (k < n); 105 | 106 | 107 | return (sign * an); 108 | } 109 | -------------------------------------------------------------------------------- /src/compress.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | std::string compress_string(const std::string& str, 4 | int compressionlevel = Z_BEST_COMPRESSION); 5 | std::string decompress_string(const std::string& str); 6 | std::string compress_string(const unsigned char * data, int length, 7 | int compressionlevel = Z_BEST_COMPRESSION); 8 | -------------------------------------------------------------------------------- /src/eigen_util.cpp: -------------------------------------------------------------------------------- 1 | #include "eigen_util.h" 2 | 3 | 4 | double unsignedQuaternionDistance(Quaternionf q1, Quaternionf q2) { 5 | float r = q1.dot(q2); 6 | if (r < -1.0 || r > 1.0) { 7 | return 0; 8 | } 9 | r = acos(r); 10 | return r <= M_PI_2? r: M_PI-r; 11 | } 12 | 13 | 14 | eePose eePosePlus(const eePose& in, const Vector3d& a) { 15 | eePose toReturn; 16 | toReturn.px = in.px + a.x(); 17 | toReturn.py = in.py + a.y(); 18 | toReturn.pz = in.pz + a.z(); 19 | toReturn.qx = in.qx; 20 | toReturn.qy = in.qy; 21 | toReturn.qz = in.qz; 22 | toReturn.qw = in.qw; 23 | return toReturn; 24 | } 25 | 26 | eePose eePoseMinus(const eePose& in, const Vector3d& a) { 27 | eePose toReturn; 28 | toReturn.px = in.px - a.x(); 29 | toReturn.py = in.py - a.y(); 30 | toReturn.pz = in.pz - a.z(); 31 | toReturn.qx = in.qx; 32 | toReturn.qy = in.qy; 33 | toReturn.qz = in.qz; 34 | toReturn.qw = in.qw; 35 | return toReturn; 36 | } 37 | -------------------------------------------------------------------------------- /src/eigen_util.h: -------------------------------------------------------------------------------- 1 | #ifndef _EIGEN_UTIL_H_ 2 | #define _EIGEN_UTIL_H_ 3 | 4 | #include 5 | #include "eePose.h" 6 | using namespace Eigen; 7 | 8 | double unsignedQuaternionDistance(Quaternionf q1, Quaternionf q2); 9 | 10 | eePose eePosePlus(const eePose&, const Vector3d& a); 11 | eePose eePoseMinus(const eePose&, const Vector3d& a); 12 | 13 | #endif // _EIGEN_UTIL_H_ 14 | -------------------------------------------------------------------------------- /src/ein_render.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "ein_words.h" 4 | #include "ein.h" 5 | #include "qtgui/einwindow.h" 6 | #include "camera.h" 7 | 8 | namespace ein_words { 9 | 10 | WORD(GuiShowAll) 11 | virtual void execute(MachineState * ms) 12 | { 13 | 14 | if (!ms->config.showgui) { 15 | return; 16 | } 17 | ms->config.dogSnoutViewWindow->setVisible(true); 18 | 19 | ms->config.rangeogramWindow->setVisible(true); 20 | 21 | ms->config.objectViewerWindow->setVisible(true); 22 | 23 | ms->config.objectMapViewerWindow->setVisible(true); 24 | 25 | ms->config.meanViewerWindow->setVisible(true); 26 | 27 | ms->config.mapBackgroundViewWindow->setVisible(true); 28 | 29 | ms->config.wristViewWindow->setVisible(true); 30 | ms->config.coreViewWindow->setVisible(true); 31 | } 32 | END_WORD 33 | REGISTER_WORD(GuiShowAll) 34 | 35 | WORD(GuiHideAll) 36 | virtual void execute(MachineState * ms) 37 | { 38 | 39 | if (!ms->config.showgui) { 40 | return; 41 | } 42 | ms->config.dogSnoutViewWindow->setVisible(false); 43 | 44 | ms->config.rangeogramWindow->setVisible(false); 45 | 46 | ms->config.objectViewerWindow->setVisible(false); 47 | 48 | ms->config.objectMapViewerWindow->setVisible(false); 49 | 50 | ms->config.meanViewerWindow->setVisible(false); 51 | 52 | ms->config.mapBackgroundViewWindow->setVisible(false); 53 | ms->config.wristViewWindow->setVisible(false); 54 | ms->config.coreViewWindow->setVisible(false); 55 | } 56 | END_WORD 57 | REGISTER_WORD(GuiHideAll) 58 | 59 | WORD(GuiCustom1) 60 | virtual void execute(MachineState * ms) 61 | { 62 | 63 | 64 | if (!ms->config.showgui) { 65 | return; 66 | } 67 | ms->config.rangeogramWindow->setVisible(false); 68 | 69 | ms->config.dogSnoutViewWindow->setVisible(false); 70 | 71 | ms->config.objectViewerWindow->setVisible(false); 72 | ms->config.objectMapViewerWindow->setVisible(false); 73 | ms->config.meanViewerWindow->setVisible(false); 74 | ms->config.mapBackgroundViewWindow->setVisible(false); 75 | ms->config.wristViewWindow->setVisible(false); 76 | ms->config.coreViewWindow->setVisible(false); 77 | ms->config.observedWindow->setVisible(false); 78 | ms->config.observedStdDevWindow->setVisible(false); 79 | ms->config.zWindow->setVisible(false); 80 | 81 | ms->config.predictedWindow->setVisible(false); 82 | ms->config.predictedStdDevWindow->setVisible(false); 83 | ms->config.backgroundWindow->setVisible(false); 84 | ms->config.backgroundWindow->setVisible(false); 85 | 86 | 87 | } 88 | 89 | END_WORD 90 | REGISTER_WORD(GuiCustom1) 91 | 92 | 93 | CONFIG_GETTER_DOUBLE(RenderWristViewBrightnessScalar, ms->config.wristViewBrightnessScalar, "") 94 | CONFIG_SETTER_DOUBLE(RenderSetWristViewBrightnessScalar, ms->config.wristViewBrightnessScalar) 95 | 96 | 97 | } 98 | -------------------------------------------------------------------------------- /src/ein_util.h: -------------------------------------------------------------------------------- 1 | #ifndef _EIN_UTIL_H_ 2 | #define _EIN_UTIL_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include "word.h" 9 | 10 | #include "base64.h" 11 | 12 | class MachineState; 13 | 14 | namespace ros { 15 | class Time; 16 | } 17 | 18 | using namespace std; 19 | using namespace cv; 20 | 21 | #define MY_FONT FONT_HERSHEY_SIMPLEX 22 | 23 | 24 | #define RANGE_UPPER_INVALID 0.3 25 | #define RANGE_LOWER_INVALID 0.08 26 | 27 | 28 | typedef enum { 29 | UNKNOWN = -1, 30 | FAILURE = 0, 31 | SUCCESS = 1 32 | } operationStatusType; 33 | 34 | 35 | std::string operationStatusToString(operationStatusType mode) ; 36 | 37 | 38 | typedef enum { 39 | UNIFORM_PRIOR, 40 | ANALYTIC_PRIOR 41 | } priorType; 42 | 43 | typedef enum { 44 | MRT, 45 | SPOON, 46 | KNIFE, 47 | OFT_INVALID 48 | } orientedFilterType; 49 | 50 | 51 | 52 | 53 | 54 | 55 | #define ORIENTATIONS 180//12 56 | #define O_FILTER_WIDTH 25//25 57 | #define O_FILTER_SPOON_HEAD_WIDTH 6 58 | #define O_FILTER_SPOON_SHAFT_WIDTH 2 59 | 60 | 61 | void pushGridSign(MachineState * ms, double speed); 62 | bool isSketchyMat(Mat sketchy); 63 | 64 | void initializeMachine(MachineState * ms); 65 | string formatTime(ros::Time time); 66 | bool copyDir(string src, string dest); 67 | 68 | string readBinaryFromYaml(FileNode & fn); 69 | void writeBinaryToYaml(unsigned char * data, int length, FileStorage & fsvO); 70 | 71 | Mat readMatFromYaml(FileNode & fsvO); 72 | void writeMatToYaml(Mat m, FileStorage & fsvO); 73 | 74 | string sceneModelFile(MachineState * ms, string label); 75 | string streamDirectory(MachineState * ms, int classIdx); 76 | 77 | string xmlEncode(const string data); 78 | 79 | vector glob(const string& pat); 80 | double deg2rad (double degrees); 81 | 82 | #endif /* _EIN_UTIL_H_ */ 83 | -------------------------------------------------------------------------------- /src/ein_words.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | 12 | 13 | #include "ein.h" 14 | 15 | #include "ein_words.h" 16 | 17 | // overall: 30s 18 | // no words; 12s 19 | // only vision, scanning, misc: 22s 20 | // only bandit, render, movement, servo: 22s 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /src/faces.cpp: -------------------------------------------------------------------------------- 1 | #include "faces.h" 2 | #include "ein_util.h" 3 | #include "eePose.h" 4 | #include 5 | 6 | 7 | string face_cascade_name = "/usr/share/opencv/haarcascades/haarcascade_frontalface_alt.xml"; 8 | string eyes_cascade_name = "/usr/share/opencv/haarcascades/haarcascade_eye_tree_eyeglasses.xml"; 9 | CascadeClassifier face_cascade; 10 | CascadeClassifier eyes_cascade; 11 | int initialized = 0; 12 | RNG rng(12345); 13 | 14 | vector faceDetectAndDisplay(string windowName, Mat frame ) { 15 | if (!initialized) { 16 | cout << "Initializing faces." << face_cascade_name << endl; 17 | if (!face_cascade.load(face_cascade_name)) { 18 | ROS_ERROR_STREAM("Error loading " << face_cascade_name); 19 | } 20 | if (!eyes_cascade.load(eyes_cascade_name)) { 21 | ROS_ERROR_STREAM("Error loading " << eyes_cascade_name); 22 | } 23 | initialized = 1; 24 | } 25 | 26 | 27 | std::vector faces; 28 | Mat frame_gray = frame.clone(); 29 | 30 | cvtColor( frame, frame_gray, CV_BGR2GRAY ); 31 | equalizeHist( frame_gray, frame_gray ); 32 | 33 | //-- Detect faces 34 | face_cascade.detectMultiScale( frame_gray, faces, 1.1, 2, 0|CV_HAAR_SCALE_IMAGE, Size(30, 30) ); 35 | 36 | for( size_t i = 0; i < faces.size(); i++ ) 37 | { 38 | Point center( faces[i].x + faces[i].width*0.5, faces[i].y + faces[i].height*0.5 ); 39 | ellipse( frame, center, Size( faces[i].width*0.5, faces[i].height*0.5), 0, 0, 360, Scalar( 255, 0, 255 ), 4, 8, 0 ); 40 | 41 | Mat faceROI = frame_gray( faces[i] ); 42 | std::vector eyes; 43 | 44 | //-- In each face, detect eyes 45 | eyes_cascade.detectMultiScale( faceROI, eyes, 1.1, 2, 0 |CV_HAAR_SCALE_IMAGE, Size(30, 30) ); 46 | 47 | for( size_t j = 0; j < eyes.size(); j++ ) 48 | { 49 | Point center( faces[i].x + eyes[j].x + eyes[j].width*0.5, faces[i].y + eyes[j].y + eyes[j].height*0.5 ); 50 | int radius = cvRound( (eyes[j].width + eyes[j].height)*0.25 ); 51 | circle( frame, center, radius, Scalar( 255, 0, 0 ), 4, 8, 0 ); 52 | } 53 | } 54 | //-- Show what you got 55 | return faces; 56 | } 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /src/faces.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef _FACES_H_ 3 | #define _FACES_H_ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace cv; 10 | 11 | vector faceDetectAndDisplay(string windowName, Mat frame ) ; 12 | #endif /* _FACES_H_ */ 13 | -------------------------------------------------------------------------------- /src/gaussian_map_words.h: -------------------------------------------------------------------------------- 1 | #ifndef _GAUSSIAN_MAP_WORDS_H_ 2 | #define _GAUSSIAN_MAP_WORDS_H_ 3 | 4 | #include "gaussian_map.h" 5 | #include "word.h" 6 | 7 | class GaussianMapWord: public Word 8 | { 9 | //private: 10 | 11 | 12 | public: 13 | 14 | shared_ptr map; 15 | GaussianMapWord(shared_ptr map); 16 | string name() { 17 | stringstream ss; 18 | ss << "GaussianMap"; 19 | return ss.str(); 20 | } 21 | 22 | string to_string() { 23 | return name(); 24 | } 25 | }; 26 | 27 | 28 | class SceneWord: public Word 29 | { 30 | //private: 31 | 32 | 33 | public: 34 | 35 | shared_ptr scene; 36 | SceneWord(shared_ptr scene); 37 | string name() { 38 | stringstream ss; 39 | ss << "Scene"; 40 | return ss.str(); 41 | } 42 | 43 | string to_string() { 44 | return name(); 45 | } 46 | }; 47 | 48 | #endif /* _GAUSSIAN_MAP_WORDS_H_ */ 49 | -------------------------------------------------------------------------------- /src/jaco/ein_jaco.h: -------------------------------------------------------------------------------- 1 | #ifndef _EIN_JACO_H_ 2 | #define _EIN_JACO_H_ 3 | 4 | class EinJacoConfig; 5 | class MachineState; 6 | void robotInitializeConfig(MachineState * ms); 7 | void robotInitializeMachine(MachineState * ms); 8 | void robotEndPointCallback(MachineState * ms); 9 | void robotSetCurrentJointPositions(MachineState * ms); 10 | 11 | void robotActivateSensorStreaming(MachineState * ms); 12 | void robotDeactivateSensorStreaming(MachineState * ms); 13 | void robotUpdate(MachineState * ms); 14 | 15 | #include 16 | #include 17 | #endif /* _EIN_JACO_H_ */ 18 | -------------------------------------------------------------------------------- /src/jaco/ein_jaco_config.h: -------------------------------------------------------------------------------- 1 | #ifndef _EIN_JACO_CONFIG_H_ 2 | #define _EIN_JACO_CONFIG_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include "ein_words.h" 12 | #include "config.h" 13 | #include "camera.h" 14 | 15 | 16 | class EinJacoConfig { 17 | public: 18 | MachineState * ms; 19 | EinJacoConfig(MachineState * ms); 20 | 21 | ros::Subscriber kinova_pose_sub; 22 | ros::Subscriber kinova_finger_pose_sub; 23 | actionlib::SimpleActionClient kinova_arm_pose_action; 24 | actionlib::SimpleActionClient kinova_fingers_position_action; 25 | 26 | ros::NodeHandle n; 27 | void endpointCallback(const geometry_msgs::PoseStamped& _p); 28 | void fingerCallback(const kinova_msgs::FingerPosition p); 29 | 30 | eePose candleLikePose; 31 | eePose calibrationPose; 32 | 33 | double finger1; 34 | double finger2; 35 | double finger3; 36 | 37 | 38 | }; 39 | 40 | #endif /* _EIN_JACO_CONFIG_H_ */ 41 | -------------------------------------------------------------------------------- /src/kuka/ein_kuka.h: -------------------------------------------------------------------------------- 1 | #ifndef _EIN_KUKA_H_ 2 | #define _EIN_KUKA_H_ 3 | 4 | class EinKukaConfig; 5 | class MachineState; 6 | void robotInitializeSerial(MachineState * ms); 7 | void robotInitializeConfig(MachineState * ms); 8 | void robotInitializeMachine(MachineState * ms); 9 | void robotEndPointCallback(MachineState * ms); 10 | void robotSetCurrentJointPositions(MachineState * ms); 11 | 12 | void robotActivateSensorStreaming(MachineState * ms); 13 | void robotDeactivateSensorStreaming(MachineState * ms); 14 | void robotUpdate(MachineState * ms); 15 | 16 | #include 17 | #include 18 | #endif /* _EIN_KUKA_H_ */ 19 | -------------------------------------------------------------------------------- /src/kuka/ein_kuka_config.h: -------------------------------------------------------------------------------- 1 | #ifndef _EIN_KUKA_CONFIG_H_ 2 | #define _EIN_KUKA_CONFIG_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | 11 | #include 12 | 13 | 14 | #include 15 | using namespace moveit; 16 | using namespace planning_interface; 17 | 18 | 19 | #include "ein_words.h" 20 | #include "config.h" 21 | #include "camera.h" 22 | 23 | 24 | 25 | class EinKukaConfig { 26 | public: 27 | MachineState * ms; 28 | EinKukaConfig(MachineState * ms); 29 | ros::NodeHandle n; 30 | 31 | 32 | 33 | 34 | MoveGroupInterface * arm; 35 | vector endEffectors; 36 | int focused_ee = 0; 37 | 38 | ros::Subscriber jointStateSubscriber; 39 | ros::Publisher cartesianPosePub; 40 | ros::Publisher gripperPub; 41 | 42 | ros::Time lastMoveitCallTime; 43 | eePose lastMoveitCallPose; 44 | 45 | const static unsigned int num_joints=7; 46 | const vector homedJoints{0,0,0,0,0,0,0}; 47 | const vector upperBodyJoints{"a1", 48 | "a2", 49 | "a3", 50 | "a4", 51 | "a5", 52 | "a6", 53 | "a7"}; 54 | 55 | 56 | double gridSize=0.01; 57 | 58 | eePose armPose; 59 | eePose armTargetPose; 60 | 61 | void changeFocusedEndEffector(int idx); 62 | void jointStateCallback(const sensor_msgs::JointState& js); 63 | void moveitStatusCallback(const actionlib_msgs::GoalStatusArray & m); 64 | }; 65 | 66 | #endif /* _EIN_KUKA_CONFIG_H_ */ 67 | -------------------------------------------------------------------------------- /src/movo/ein_movo.h: -------------------------------------------------------------------------------- 1 | #ifndef _EIN_MOVO_H_ 2 | #define _EIN_MOVO_H_ 3 | 4 | class EinMovoConfig; 5 | class MachineState; 6 | void robotInitializeSerial(MachineState * ms); 7 | void robotInitializeConfig(MachineState * ms); 8 | void robotInitializeMachine(MachineState * ms); 9 | void robotEndPointCallback(MachineState * ms); 10 | void robotSetCurrentJointPositions(MachineState * ms); 11 | 12 | void robotActivateSensorStreaming(MachineState * ms); 13 | void robotDeactivateSensorStreaming(MachineState * ms); 14 | void robotUpdate(MachineState * ms); 15 | 16 | #include 17 | #include 18 | #endif /* _EIN_MOVO_H_ */ 19 | -------------------------------------------------------------------------------- /src/opencl/oclNbody/Makefile: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # 3 | # Copyright 1993-2009 NVIDIA Corporation. All rights reserved. 4 | # 5 | # NOTICE TO USER: 6 | # 7 | # This source code is subject to NVIDIA ownership rights under U.S. and 8 | # international Copyright laws. 9 | # 10 | # NVIDIA MAKES NO REPRESENTATION ABOUT THE SUITABILITY OF THIS SOURCE 11 | # CODE FOR ANY PURPOSE. IT IS PROVIDED "AS IS" WITHOUT EXPRESS OR 12 | # IMPLIED WARRANTY OF ANY KIND. NVIDIA DISCLAIMS ALL WARRANTIES WITH 13 | # REGARD TO THIS SOURCE CODE, INCLUDING ALL IMPLIED WARRANTIES OF 14 | # MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE. 15 | # IN NO EVENT SHALL NVIDIA BE LIABLE FOR ANY SPECIAL, INDIRECT, INCIDENTAL, 16 | # OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS 17 | # OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE 18 | # OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE 19 | # OR PERFORMANCE OF THIS SOURCE CODE. 20 | # 21 | # U.S. Government End Users. This source code is a "commercial item" as 22 | # that term is defined at 48 C.F.R. 2.101 (OCT 1995), consisting of 23 | # "commercial computer software" and "commercial computer software 24 | # documentation" as such terms are used in 48 C.F.R. 12.212 (SEPT 1995) 25 | # and is provided to the U.S. Government only as a commercial end item. 26 | # Consistent with 48 C.F.R.12.212 and 48 C.F.R. 227.7202-1 through 27 | # 227.7202-4 (JUNE 1995), all U.S. Government End Users acquire the 28 | # source code with only those rights set forth herein. 29 | # 30 | ################################################################################ 31 | # 32 | # Build script for project 33 | # 34 | ################################################################################ 35 | 36 | # Add source files here 37 | EXECUTABLE := oclNbody 38 | # C/C++ source files (compiled with gcc / c++) 39 | SRCDIR := src/ 40 | CCFILES := oclBodySystemCpu.cpp oclBodySystemOpencl.cpp oclBodySystemOpenclLaunch.cpp oclRenderParticles.cpp oclNbody.cpp oclNbodyGold.cpp paramgl.cpp param.cpp shrUtils.cpp cmd_arg_reader.cpp oclUtils.cpp 41 | INCDIR := inc/ 42 | 43 | USEGLLIB := 1 44 | USEGLUT := 1 45 | 46 | ################################################################################ 47 | # Rules and targets 48 | 49 | # NOTE: Assuming P4 based install until OpenCL becomes public 50 | # Adjust P4_Root, default is ${HOME}/perforce/ 51 | # P4_ROOT=${HOME}/myperforce/ 52 | 53 | include ../../common/common_opencl.mk 54 | 55 | 56 | -------------------------------------------------------------------------------- /src/opencl/oclNbody/inc/oclBodySystem.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 1993-2010 NVIDIA Corporation. All rights reserved. 3 | * 4 | * Please refer to the NVIDIA end user license agreement (EULA) associated 5 | * with this source code for terms and conditions that govern your use of 6 | * this software. Any use, reproduction, disclosure, or distribution of 7 | * this software and related documentation outside the terms of the EULA 8 | * is strictly prohibited. 9 | * 10 | */ 11 | 12 | #ifndef __CLH_BODYSYSTEM_H__ 13 | #define __CLH_BODYSYSTEM_H__ 14 | 15 | #include 16 | 17 | enum NBodyConfig 18 | { 19 | NBODY_CONFIG_RANDOM, 20 | NBODY_CONFIG_SHELL, 21 | NBODY_CONFIG_EXPAND, 22 | NBODY_NUM_CONFIGS 23 | }; 24 | 25 | // utility function 26 | void randomizeBodies(NBodyConfig config, float* pos, float* vel, float* color, float clusterScale, 27 | float velocityScale, int numBodies); 28 | 29 | // BodySystem abstract base class 30 | class BodySystem 31 | { 32 | public: // methods 33 | BodySystem(int numBodies) : m_numBodies(numBodies), m_bInitialized(false) {} 34 | virtual ~BodySystem() {} 35 | 36 | virtual void update(float deltaTime) = 0; 37 | 38 | enum BodyArray 39 | { 40 | BODYSYSTEM_POSITION, 41 | BODYSYSTEM_VELOCITY, 42 | }; 43 | 44 | virtual void setSoftening(float softening) = 0; 45 | virtual void setDamping(float damping) = 0; 46 | 47 | virtual float* getArray(BodyArray array) = 0; 48 | virtual void setArray(BodyArray array, const float* data) = 0; 49 | 50 | virtual size_t getCurrentReadBuffer() const = 0; 51 | 52 | virtual int getNumBodies() const { return m_numBodies; } 53 | 54 | virtual void synchronizeThreads() const {}; 55 | 56 | protected: // methods 57 | BodySystem() {} // default constructor 58 | 59 | virtual void _initialize(int numBodies) = 0; 60 | virtual void _finalize() = 0; 61 | 62 | protected: // data 63 | int m_numBodies; 64 | bool m_bInitialized; 65 | }; 66 | 67 | #endif // __CLH_BODYSYSTEM_H__ 68 | -------------------------------------------------------------------------------- /src/opencl/oclNbody/inc/oclBodySystemCpu.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 1993-2010 NVIDIA Corporation. All rights reserved. 3 | * 4 | * Please refer to the NVIDIA end user license agreement (EULA) associated 5 | * with this source code for terms and conditions that govern your use of 6 | * this software. Any use, reproduction, disclosure, or distribution of 7 | * this software and related documentation outside the terms of the EULA 8 | * is strictly prohibited. 9 | * 10 | */ 11 | 12 | #ifndef __CL_BODYSYSTEMCPU_H__ 13 | #define __CL_BODYSYSTEMCPU_H__ 14 | 15 | #include "oclBodySystem.h" 16 | 17 | // CPU Body System 18 | class BodySystemCPU : public BodySystem 19 | { 20 | public: 21 | BodySystemCPU(int numBodies); 22 | virtual ~BodySystemCPU(); 23 | 24 | virtual void update(float deltaTime); 25 | 26 | virtual void setSoftening(float softening) { m_softeningSquared = softening * softening; } 27 | virtual void setDamping(float damping) { m_damping = damping; } 28 | 29 | virtual float* getArray(BodyArray array); 30 | virtual void setArray(BodyArray array, const float* data); 31 | 32 | virtual size_t getCurrentReadBuffer() const { return m_currentRead; } 33 | 34 | protected: // methods 35 | BodySystemCPU() {} // default constructor 36 | 37 | virtual void _initialize(int numBodies); 38 | virtual void _finalize(); 39 | 40 | void _computeNBodyGravitation(); 41 | void _integrateNBodySystem(float deltaTime); 42 | 43 | protected: // data 44 | float* m_pos[2]; 45 | float* m_vel[2]; 46 | float* m_force; 47 | 48 | float m_softeningSquared; 49 | float m_damping; 50 | 51 | unsigned int m_currentRead; 52 | unsigned int m_currentWrite; 53 | }; 54 | 55 | #endif // __CL_BODYSYSTEMCPU_H__ 56 | -------------------------------------------------------------------------------- /src/opencl/oclNbody/inc/oclBodySystemOpencl.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 1993-2010 NVIDIA Corporation. All rights reserved. 3 | * 4 | * Please refer to the NVIDIA end user license agreement (EULA) associated 5 | * with this source code for terms and conditions that govern your use of 6 | * this software. Any use, reproduction, disclosure, or distribution of 7 | * this software and related documentation outside the terms of the EULA 8 | * is strictly prohibited. 9 | * 10 | */ 11 | 12 | #ifndef __CL_BODYSYSTEMOPENCL_H__ 13 | #define __CL_BODYSYSTEMOPENCL_H__ 14 | 15 | #if defined (__APPLE__) || defined(MACOSX) 16 | #include 17 | #else 18 | #include 19 | #endif 20 | #include "oclBodySystem.h" 21 | 22 | // OpenCL BodySystem: runs on the GPU 23 | class BodySystemOpenCL : public BodySystem 24 | { 25 | public: 26 | BodySystemOpenCL(int numBodies, cl_device_id dev, cl_context ctx, cl_command_queue cmdq, unsigned int p, unsigned int q, bool usePBO, bool bDouble); 27 | virtual ~BodySystemOpenCL(); 28 | 29 | virtual void update(float deltaTime); 30 | 31 | virtual void setSoftening(float softening); 32 | virtual void setDamping(float damping); 33 | 34 | virtual float* getArray(BodyArray array); 35 | virtual void setArray(BodyArray array, const float* data); 36 | 37 | virtual size_t getCurrentReadBuffer() const 38 | { 39 | if (m_bUsePBO) 40 | { 41 | return m_pboGL[m_currentRead]; 42 | } 43 | else 44 | { 45 | return (size_t) m_hPos; 46 | } 47 | } 48 | 49 | virtual void synchronizeThreads() const; 50 | 51 | protected: // methods 52 | BodySystemOpenCL() {} 53 | 54 | virtual void _initialize(int numBodies); 55 | virtual void _finalize(); 56 | 57 | protected: // data 58 | cl_device_id device; 59 | cl_context cxContext; 60 | cl_command_queue cqCommandQueue; 61 | 62 | cl_kernel MT_kernel; 63 | cl_kernel noMT_kernel; 64 | 65 | // CPU data 66 | float* m_hPos; 67 | float* m_hVel; 68 | 69 | // GPU data 70 | cl_mem m_dPos[2]; 71 | cl_mem m_dVel[2]; 72 | 73 | bool m_bUsePBO; 74 | 75 | float m_softeningSq; 76 | float m_damping; 77 | 78 | unsigned int m_pboGL[2]; 79 | cl_mem m_pboCL[2]; 80 | unsigned int m_currentRead; 81 | unsigned int m_currentWrite; 82 | 83 | unsigned int m_p; 84 | unsigned int m_q; 85 | 86 | //for double precision 87 | bool m_bDouble; 88 | }; 89 | 90 | #endif // __CLH_BODYSYSTEMOPENCL_H__ 91 | -------------------------------------------------------------------------------- /src/opencl/oclNbody/inc/oclBodySystemOpenclLaunch.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 1993-2010 NVIDIA Corporation. All rights reserved. 3 | * 4 | * Please refer to the NVIDIA end user license agreement (EULA) associated 5 | * with this source code for terms and conditions that govern your use of 6 | * this software. Any use, reproduction, disclosure, or distribution of 7 | * this software and related documentation outside the terms of the EULA 8 | * is strictly prohibited. 9 | * 10 | */ 11 | 12 | #ifndef __CL_BODYSYSTEMOPENCL_LAUNCH_H 13 | #define __CL_BODYSYSTEMOPENCL_LAUNCH_H 14 | 15 | #ifdef __cplusplus 16 | extern "C" 17 | { 18 | #endif 19 | 20 | #include 21 | 22 | #if defined (__APPLE__) || defined(MACOSX) 23 | #include 24 | #else 25 | #include 26 | #endif 27 | 28 | #include "oclBodySystemOpencl.h" 29 | 30 | int CreateProgramAndKernel(cl_context ctx, cl_device_id* cdDevices, const char* kernel_name, cl_kernel* kernel, bool bDouble); 31 | void AllocateNBodyArrays(cl_context ctx, cl_mem* vel, int numBodies, int dFlag); 32 | void DeleteNBodyArrays(cl_mem* vel); 33 | 34 | 35 | void IntegrateNbodySystem(cl_command_queue cqCommandQueue, 36 | cl_kernel MT_kernel, cl_kernel noMT_kernel, 37 | cl_mem newPos, cl_mem newVel, 38 | cl_mem oldPos, cl_mem oldVel, 39 | cl_mem pboCLOldPos, cl_mem pboCLNewPos, 40 | float deltaTime, float damping, float softSq, 41 | int numBodies, int p, int q, 42 | int bUsePBO, bool bDouble); 43 | void CopyArrayFromDevice(cl_command_queue cmdq, float *host, cl_mem device, cl_mem pboCL, int numBodies, bool bDouble); 44 | void CopyArrayToDevice(cl_command_queue cmdq, cl_mem device, const float *host, int numBodies, bool bDouble); 45 | cl_mem RegisterGLBufferObject(cl_context ctx, unsigned int pboGL); 46 | void UnregisterGLBufferObject(cl_mem pboCL); 47 | void ThreadSync(cl_command_queue cmdq); 48 | 49 | #ifdef __cplusplus 50 | } 51 | #endif 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /src/opencl/oclNbody/inc/oclRenderParticles.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 1993-2010 NVIDIA Corporation. All rights reserved. 3 | * 4 | * Please refer to the NVIDIA end user license agreement (EULA) associated 5 | * with this source code for terms and conditions that govern your use of 6 | * this software. Any use, reproduction, disclosure, or distribution of 7 | * this software and related documentation outside the terms of the EULA 8 | * is strictly prohibited. 9 | * 10 | */ 11 | 12 | #ifndef __RENDER_PARTICLES__ 13 | #define __RENDER_PARTICLES__ 14 | 15 | class ParticleRenderer 16 | { 17 | public: 18 | ParticleRenderer(); 19 | ~ParticleRenderer(); 20 | 21 | void setPositions(float *pos, int numParticles); 22 | void setColors(float *color, int numParticles); 23 | void setPBO(size_t pbo, int numParticles); 24 | 25 | enum DisplayMode 26 | { 27 | PARTICLE_POINTS, 28 | PARTICLE_SPRITES, 29 | PARTICLE_SPRITES_COLOR, 30 | PARTICLE_NUM_MODES 31 | }; 32 | 33 | void display(DisplayMode mode = PARTICLE_POINTS); 34 | 35 | void setPointSize(float size) { m_pointSize = size; } 36 | void setSpriteSize(float size) { m_spriteSize = size; } 37 | 38 | protected: // methods 39 | void _initGL(); 40 | void _createTexture(int resolution); 41 | void _drawPoints(bool color = false); 42 | 43 | protected: // data 44 | float *m_pos; 45 | int m_numParticles; 46 | 47 | float m_pointSize; 48 | float m_spriteSize; 49 | 50 | unsigned int m_vertexShader; 51 | unsigned int m_pixelShader; 52 | unsigned int m_program; 53 | unsigned int m_texture; 54 | size_t m_pbo; 55 | unsigned int m_vboColor; 56 | }; 57 | 58 | #endif //__ RENDER_PARTICLES__ 59 | -------------------------------------------------------------------------------- /src/opencl/oclNbody/inc/paramgl.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 1993-2010 NVIDIA Corporation. All rights reserved. 3 | * 4 | * Please refer to the NVIDIA end user license agreement (EULA) associated 5 | * with this source code for terms and conditions that govern your use of 6 | * this software. Any use, reproduction, disclosure, or distribution of 7 | * this software and related documentation outside the terms of the EULA 8 | * is strictly prohibited. 9 | * 10 | */ 11 | 12 | /* 13 | ParamListGL 14 | - class derived from ParamList to do simple OpenGL rendering of a parameter list 15 | */ 16 | 17 | #ifndef PARAMGL_H 18 | #define PARAMGL_H 19 | 20 | #if defined(__APPLE__) || defined(MACOSX) 21 | #include 22 | #else 23 | #include 24 | #endif 25 | 26 | #include 27 | 28 | void beginWinCoords(); 29 | void endWinCoords(); 30 | void glPrint(int x, int y, const char *s, void *font); 31 | void glPrintShadowed(int x, int y, const char *s, void *font, float *color); 32 | 33 | class ParamListGL : public ParamList { 34 | public: 35 | ParamListGL(char *name = ""); 36 | 37 | void Render(int x, int y, bool shadow = false); 38 | bool Mouse(int x, int y, int button=GLUT_LEFT_BUTTON, int state=GLUT_DOWN); 39 | bool Motion(int x, int y); 40 | void Special(int key, int x, int y); 41 | 42 | void SetSelectedColor(float r, float g, float b) { text_col_selected[0] = r; text_col_selected[1] = g; text_col_selected[2] = b; } 43 | void SetUnSelectedColor(float r, float g, float b) { text_col_unselected[0] = r; text_col_unselected[1] = g; text_col_unselected[2] = b; } 44 | 45 | int bar_x; 46 | int bar_w; 47 | int bar_h; 48 | int text_x; 49 | int separation; 50 | int value_x; 51 | int font_h; 52 | int start_x, start_y; 53 | int bar_offset; 54 | 55 | float text_col_selected[3]; 56 | float text_col_unselected[3]; 57 | float text_col_shadow[3]; 58 | float bar_col_outer[3]; 59 | float bar_col_inner[3]; 60 | 61 | void *font; 62 | }; 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /src/opencl/oclNbody/obj/release/cmd_arg_reader.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/src/opencl/oclNbody/obj/release/cmd_arg_reader.cpp.o -------------------------------------------------------------------------------- /src/opencl/oclNbody/obj/release/oclBodySystemCpu.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/src/opencl/oclNbody/obj/release/oclBodySystemCpu.cpp.o -------------------------------------------------------------------------------- /src/opencl/oclNbody/obj/release/oclBodySystemOpencl.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/src/opencl/oclNbody/obj/release/oclBodySystemOpencl.cpp.o -------------------------------------------------------------------------------- /src/opencl/oclNbody/obj/release/oclBodySystemOpenclLaunch.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/src/opencl/oclNbody/obj/release/oclBodySystemOpenclLaunch.cpp.o -------------------------------------------------------------------------------- /src/opencl/oclNbody/obj/release/oclNbody.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/src/opencl/oclNbody/obj/release/oclNbody.cpp.o -------------------------------------------------------------------------------- /src/opencl/oclNbody/obj/release/oclNbodyGold.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/src/opencl/oclNbody/obj/release/oclNbodyGold.cpp.o -------------------------------------------------------------------------------- /src/opencl/oclNbody/obj/release/oclRenderParticles.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/src/opencl/oclNbody/obj/release/oclRenderParticles.cpp.o -------------------------------------------------------------------------------- /src/opencl/oclNbody/obj/release/oclUtils.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/src/opencl/oclNbody/obj/release/oclUtils.cpp.o -------------------------------------------------------------------------------- /src/opencl/oclNbody/obj/release/param.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/src/opencl/oclNbody/obj/release/param.cpp.o -------------------------------------------------------------------------------- /src/opencl/oclNbody/obj/release/paramgl.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/src/opencl/oclNbody/obj/release/paramgl.cpp.o -------------------------------------------------------------------------------- /src/opencl/oclNbody/obj/release/shrUtils.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/src/opencl/oclNbody/obj/release/shrUtils.cpp.o -------------------------------------------------------------------------------- /src/opencl/oclNbody/oclNbody.ptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/h2r/ein/4619d58d498bcf6720a3122a6b98f480ce460efa/src/opencl/oclNbody/oclNbody.ptx -------------------------------------------------------------------------------- /src/opencl/oclNbody/oclNbody.txt: -------------------------------------------------------------------------------- 1 | /home/stefie10/dev/opencl/NVIDIA GPU Computing SDK/OpenCL/bin/linux/debug/oclNbody Starting... 2 | 3 | Command line switches: 4 | --qatest Check correctness of GPU execution and measure performance) 5 | --noprompt Quit simulation automatically after a brief period 6 | --n= Specify # of bodies to simulate (default = 7680) 7 | --double Use double precision floating point values for simulation 8 | --p= Specify X dimension of workgroup (default = 256) 9 | --q= Specify Y dimension of workgroup (default = 1) 10 | 11 | clGetPlatformID... 12 | 13 | Single precision execution... 14 | 15 | Get the Device info and select Device... 16 | # of Devices Available = 1 17 | Using Device 0, GeForce GT 750M # of Compute Units = 2 18 | clCreateContext... 19 | clCreateCommandQueue... 20 | 21 | Number of Bodies = 7680 22 | Workgroup Dims = (256 x 1) 23 | 24 | Calling InitGL... 25 | 26 | AllocateNBodyArrays m_dPos 27 | 28 | AllocateNBodyArrays m_dVel 29 | 30 | CreateProgramAndKernel _noMT... 31 | Loading Uncompiled kernel from .cl file, using oclNbodyKernel.cl 32 | clCreateProgramWithSource 33 | clBuildProgram 34 | clCreateKernel 35 | 36 | CreateProgramAndKernel _MT... 37 | Loading Uncompiled kernel from .cl file, using oclNbodyKernel.cl 38 | clCreateProgramWithSource 39 | clBuildProgram 40 | clCreateKernel 41 | 42 | Reset Nbody System... 43 | 44 | Running standard oclNbody simulation... 45 | 46 | -------------------------------------------------------------------------------- /src/opencl/oclNbody/src/oclNbodyGold.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 1993-2010 NVIDIA Corporation. All rights reserved. 3 | * 4 | * Please refer to the NVIDIA end user license agreement (EULA) associated 5 | * with this source code for terms and conditions that govern your use of 6 | * this software. Any use, reproduction, disclosure, or distribution of 7 | * this software and related documentation outside the terms of the EULA 8 | * is strictly prohibited. 9 | * 10 | */ 11 | 12 | #include 13 | 14 | //////////////////////////////////////////////////////////////////////////////// 15 | // export C interface 16 | extern "C" void computeGold( float* reference, float* idata, const unsigned int len); 17 | 18 | void bodyBodyInteraction(float accel[3], float posMass0[4], float posMass1[4], float softeningSquared) 19 | { 20 | float r[3]; 21 | 22 | // r_01 [3 FLOPS] 23 | r[0] = posMass0[0] - posMass1[0]; 24 | r[1] = posMass0[1] - posMass1[1]; 25 | r[2] = posMass0[2] - posMass1[2]; 26 | 27 | // d^2 + e^2 [6 FLOPS] 28 | float distSqr = r[0] * r[0] + r[1] * r[1] + r[2] * r[2]; 29 | distSqr += softeningSquared; 30 | 31 | // invDistCube =1/distSqr^(3/2) [4 FLOPS (2 mul, 1 sqrt, 1 inv)] 32 | float invDist = 1.0f / sqrtf(distSqr); 33 | float invDistCube = invDist * invDist * invDist; 34 | 35 | // s = m_j * invDistCube [1 FLOP] 36 | float s = posMass1[3] * invDistCube; 37 | 38 | // (m_1 * r_01) / (d^2 + e^2)^(3/2) [6 FLOPS] 39 | accel[0] += r[0] * s; 40 | accel[1] += r[1] * s; 41 | accel[2] += r[2] * s; 42 | } 43 | 44 | //////////////////////////////////////////////////////////////////////////////// 45 | //! Compute reference data set 46 | //! Each element is multiplied with the number of threads / array length 47 | //! @param reference reference data, computed but preallocated 48 | //! @param idata input data as provided to device 49 | //! @param len number of elements in reference / idata 50 | //////////////////////////////////////////////////////////////////////////////// 51 | void computeGold( float* force, float* pos, const unsigned int numBodies, float softeningSquared) 52 | { 53 | for(unsigned int i = 0; i < numBodies; ++i) 54 | { 55 | force[i*4 ] = 0; 56 | force[i*4+1] = 0; 57 | force[i*4+2] = 0; 58 | force[i*4+3] = 0; 59 | } 60 | 61 | for(unsigned int i = 0; i < numBodies; ++i) 62 | { 63 | for(unsigned int j = 0; j < numBodies; ++j) 64 | { 65 | float f[4]; 66 | bodyBodyInteraction(f, &pos[j*4], &pos[i*4], softeningSquared); 67 | for (int k = 0; k < 3; ++k) 68 | { 69 | force[i*4+k] += f[k]; 70 | } 71 | } 72 | } 73 | } 74 | 75 | -------------------------------------------------------------------------------- /src/opencl/oclNbody/src/oclNbodyMain.cpp: -------------------------------------------------------------------------------- 1 | 2 | int runFunction(int argc, char** argv); 3 | 4 | int main(int argc, char** argv) { 5 | runFunction(argc, argv); 6 | } 7 | -------------------------------------------------------------------------------- /src/opencl/oclNbody/src/param.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 1993-2010 NVIDIA Corporation. All rights reserved. 3 | * 4 | * Please refer to the NVIDIA end user license agreement (EULA) associated 5 | * with this source code for terms and conditions that govern your use of 6 | * this software. Any use, reproduction, disclosure, or distribution of 7 | * this software and related documentation outside the terms of the EULA 8 | * is strictly prohibited. 9 | * 10 | */ 11 | 12 | #include 13 | 14 | const Param dummy("error"); 15 | -------------------------------------------------------------------------------- /src/pidrone/ein_pidrone.h: -------------------------------------------------------------------------------- 1 | #ifndef _EIN_PIDRONE_H_ 2 | #define _EIN_PIDRONE_H_ 3 | 4 | class EinPidroneConfig; 5 | class MachineState; 6 | void robotInitializeConfig(MachineState * ms); 7 | void robotInitializeMachine(MachineState * ms); 8 | void robotEndPointCallback(MachineState * ms); 9 | void robotSetCurrentJointPositions(MachineState * ms); 10 | void robotInitializeSerial(MachineState * ms); 11 | 12 | void robotActivateSensorStreaming(MachineState * ms); 13 | void robotDeactivateSensorStreaming(MachineState * ms); 14 | void robotUpdate(MachineState * ms); 15 | 16 | #include 17 | #include 18 | #endif /* _EIN_PIDRONE_H_ */ 19 | -------------------------------------------------------------------------------- /src/pidrone/ein_pidrone_config.h: -------------------------------------------------------------------------------- 1 | #ifndef _EIN_PIDRONE_CONFIG_H_ 2 | #define _EIN_PIDRONE_CONFIG_H_ 3 | 4 | #include 5 | 6 | class EinPidroneConfig { 7 | 8 | public: 9 | MachineState * ms; 10 | 11 | ros::Subscriber eeRanger; 12 | 13 | EinPidroneConfig(MachineState * ms); 14 | 15 | ros::Time lastMapLookupPrintTime; 16 | 17 | void endPointCallback(const ros::TimerEvent&); 18 | ros::Timer eeTimer; 19 | 20 | ros::Publisher modePub; 21 | ros::Publisher resetTransformPub; 22 | ros::Publisher toggleTransformPub; 23 | 24 | 25 | ros::NodeHandle n; 26 | }; 27 | 28 | #endif /* _EIN_PIDRONE_CONFIG_H_ */ 29 | -------------------------------------------------------------------------------- /src/qtgui/armwidget.cpp: -------------------------------------------------------------------------------- 1 | #include "armwidget.h" 2 | 3 | #include "config.h" 4 | 5 | void pilotCallbackFunc(int event, int x, int y, int flags, void* userdata); 6 | 7 | ArmWidget::ArmWidget(QWidget * parent, MachineState * _ms) : QWidget(parent), 8 | ms(_ms), 9 | ui(new Ui::ArmWidget), 10 | wristView(this, EIN_WINDOW_KEEPRATIO) 11 | { 12 | ui->setupUi(this); 13 | ui->wristViewFrame->layout()->addWidget(wristView.getWidget()); 14 | 15 | captureLineEdit = new CaptureLineEdit(); 16 | 17 | ui->captureLineEditFrame->layout()->addWidget(captureLineEdit); 18 | 19 | if (ms == NULL) { 20 | return; 21 | } 22 | ms->config.armWidget = this; 23 | 24 | callStackModel = new StackModel(this); 25 | callStackModel->setStack(ms->call_stack); 26 | ui->callStackTableView->setModel(callStackModel); 27 | ui->callStackTableView->horizontalHeader()->setSectionResizeMode(QHeaderView::ResizeToContents); 28 | 29 | dataStackModel = new StackModel(this); 30 | dataStackModel->setStack(ms->data_stack); 31 | ui->dataStackTableView->setModel(dataStackModel); 32 | ui->dataStackTableView->horizontalHeader()->setSectionResizeMode(QHeaderView::ResizeToContents); 33 | 34 | 35 | wristView.setMouseCallBack(pilotCallbackFunc, ms); 36 | 37 | QStringList wordList; 38 | std::vector > words = register_word(NULL); 39 | 40 | for (int i = 0; i < words.size(); i++) { 41 | wordList.push_back(QString::fromStdString(words[i]->name())); 42 | } 43 | completer = new QCompleter(wordList); 44 | ui->replEdit->setCompleter(completer); 45 | 46 | connect(ui->replEdit, SIGNAL(returnPressed()), this, SLOT(replReturnPressed())); 47 | 48 | } 49 | 50 | 51 | void ArmWidget::replReturnPressed() { 52 | string text = ui->replEdit->text().toStdString(); 53 | cout << "Text: " << text << endl; 54 | ui->replEdit->setText(""); 55 | ms->evaluateProgram(text); 56 | } 57 | 58 | 59 | void ArmWidget::update() { 60 | if (ms == NULL) { 61 | return; 62 | } 63 | if (captureLineEdit->last_key != -1) { 64 | ms->config.last_key = captureLineEdit->last_key; 65 | captureLineEdit->last_key = -1; 66 | } 67 | ui->stateLabel->setText(QString::fromStdString(ms->currentState())); 68 | callStackModel->setStack(ms->call_stack); 69 | dataStackModel->setStack(ms->data_stack); 70 | if ( !isSketchyMat(ms->config.wristViewImage)) { 71 | wristView.updateImage(ms->config.wristViewImage); 72 | } 73 | } 74 | -------------------------------------------------------------------------------- /src/qtgui/armwidget.h: -------------------------------------------------------------------------------- 1 | #ifndef ARMWIDGET_H 2 | #define ARMWIDGET_H 3 | 4 | #include 5 | 6 | class MachineState; 7 | 8 | #include "window_QT.h" 9 | #include "stackmodel.h" 10 | 11 | #include "ui_armwidget.h" 12 | 13 | #include "capturelineedit.h" 14 | 15 | class ArmWidget : public QWidget 16 | { 17 | Q_OBJECT 18 | 19 | public: 20 | ArmWidget(QWidget * parent, MachineState * ms); 21 | void update(); 22 | DefaultEinViewPort wristView; 23 | 24 | public slots: 25 | void replReturnPressed(); 26 | 27 | private: 28 | Ui::ArmWidget *ui; 29 | StackModel * dataStackModel; 30 | StackModel * callStackModel; 31 | 32 | CaptureLineEdit * captureLineEdit; 33 | MachineState * ms; 34 | 35 | QCompleter * completer; 36 | 37 | }; 38 | 39 | 40 | #endif // ARMWIDGET_H 41 | -------------------------------------------------------------------------------- /src/qtgui/capturelineedit.cpp: -------------------------------------------------------------------------------- 1 | #include "capturelineedit.h" 2 | 3 | #include 4 | #include 5 | using namespace std; 6 | 7 | void CaptureLineEdit::keyPressEvent(QKeyEvent * event) 8 | { 9 | QWidget::keyPressEvent(event); 10 | 11 | int key = event->key(); 12 | 13 | Qt::Key qtkey = static_cast(key); 14 | char asciiCode = QTest::keyToAscii(qtkey); 15 | if (asciiCode != 0) { 16 | key = static_cast(asciiCode); 17 | } else { 18 | key = event->nativeVirtualKey(); //same codes as returned by GTK-based backend 19 | } 20 | key = event->nativeVirtualKey(); 21 | 22 | int modifiers = event->nativeModifiers(); 23 | key |= modifiers << 16; 24 | 25 | //control plus (Z, +, -, up, down, left, right) are used for zoom/panning functions 26 | if (event->modifiers() != Qt::ControlModifier) 27 | { 28 | last_key = key; 29 | cout << "Got key: " << last_key << endl; 30 | } 31 | } 32 | 33 | -------------------------------------------------------------------------------- /src/qtgui/capturelineedit.h: -------------------------------------------------------------------------------- 1 | #ifndef CAPTURELINEEDIT_H 2 | #define CAPTURELINEEDIT_H 3 | 4 | #include 5 | #include 6 | 7 | class CaptureLineEdit : public QLineEdit 8 | { 9 | Q_OBJECT 10 | 11 | public: 12 | int last_key; 13 | void keyPressEvent(QKeyEvent * event); 14 | 15 | }; 16 | 17 | 18 | #endif // CAPTURELINEEDIT_H 19 | -------------------------------------------------------------------------------- /src/qtgui/discrepancywindow.cpp: -------------------------------------------------------------------------------- 1 | #include "discrepancywindow.h" 2 | 3 | #include "ui_discrepancywindow.h" 4 | 5 | #include 6 | #include 7 | 8 | #include "einwindow.h" 9 | #include "qt_util.h" 10 | 11 | DiscrepancyWindow::DiscrepancyWindow(QWidget *parent, MachineState * _ms) : 12 | QMainWindow(parent), 13 | ui(new Ui::DiscrepancyWindow), 14 | discrepancyView(this, EIN_WINDOW_KEEPRATIO), 15 | densityView(this, EIN_WINDOW_KEEPRATIO) 16 | { 17 | ui->setupUi(this); 18 | ms = _ms; 19 | ui->menubar->setVisible(true); 20 | connect(ui->actionSaveImage, SIGNAL(triggered()), this, SLOT(saveImage())); 21 | 22 | ui->tabs->widget(0)->layout()->addWidget(discrepancyView.getWidget()); 23 | ui->tabs->widget(1)->layout()->addWidget(densityView.getWidget()); 24 | 25 | 26 | 27 | 28 | } 29 | 30 | 31 | DiscrepancyWindow::~DiscrepancyWindow() 32 | { 33 | delete ui; 34 | } 35 | 36 | 37 | 38 | void DiscrepancyWindow::update(Mat & _discrepancyImage, Mat & _densityImage) 39 | { 40 | //widget->updateMap(map); 41 | discrepancyImage = _discrepancyImage; 42 | densityImage = _densityImage; 43 | discrepancyView.updateImage(discrepancyImage); 44 | densityView.updateImage(densityImage); 45 | } 46 | 47 | 48 | void DiscrepancyWindow::saveImage() { 49 | if (ui->tabs->currentIndex() == 0) { 50 | doSaveImage(ms, this, discrepancyImage); 51 | } else if (ui->tabs->currentIndex() == 1) { 52 | doSaveImage(ms, this, densityImage); 53 | } else { 54 | cout << "Bad index: " << ui->tabs->currentIndex() << endl; 55 | assert(0); 56 | } 57 | 58 | 59 | } 60 | 61 | 62 | void DiscrepancyWindow::toggleVisible(bool show) { 63 | setVisible(show); 64 | } 65 | 66 | void DiscrepancyWindow::setVisible(bool show) { 67 | QMainWindow::setVisible(show); 68 | emit visibleChanged(show); 69 | } 70 | -------------------------------------------------------------------------------- /src/qtgui/discrepancywindow.h: -------------------------------------------------------------------------------- 1 | #ifndef DISCREPANCYWINDOW_H 2 | #define DISCREPANCYWINDOW_H 3 | 4 | #include 5 | #include 6 | 7 | class MachineState; 8 | 9 | #include "window_QT.h" 10 | 11 | using namespace std; 12 | 13 | namespace Ui { 14 | class DiscrepancyWindow; 15 | } 16 | 17 | class DiscrepancyWindow : public QMainWindow 18 | { 19 | Q_OBJECT 20 | 21 | public: 22 | explicit DiscrepancyWindow(QWidget *parent, MachineState * _ms); 23 | ~DiscrepancyWindow(); 24 | 25 | void setWindowTitle(string s) { 26 | QMainWindow::setWindowTitle(QString::fromStdString(s)); 27 | } 28 | void update(Mat & discrepancyImage, Mat & densityImage); 29 | void setVisible(bool); 30 | 31 | public slots: 32 | void saveImage(); 33 | void toggleVisible(bool show); 34 | 35 | signals: 36 | void visibleChanged(bool visible); 37 | 38 | 39 | private: 40 | Ui::DiscrepancyWindow *ui; 41 | MachineState * ms; 42 | DefaultEinViewPort discrepancyView; 43 | DefaultEinViewPort densityView; 44 | 45 | Mat discrepancyImage; 46 | Mat densityImage; 47 | 48 | }; 49 | 50 | #endif // DISCREPANCYWINDOW_H 51 | -------------------------------------------------------------------------------- /src/qtgui/discrepancywindow.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | DiscrepancyWindow 4 | 5 | 6 | 7 | 0 8 | 0 9 | 800 10 | 600 11 | 12 | 13 | 14 | Discrepancy Viewer 15 | 16 | 17 | 18 | 19 | 20 | 21 | 0 22 | 23 | 24 | 25 | Discrepancy RGB 26 | 27 | 28 | 29 | 30 | 31 | Discrepancy Density 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 0 43 | 0 44 | 800 45 | 25 46 | 47 | 48 | 49 | 50 | File 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | Save Image 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /src/qtgui/einwindow.cpp: -------------------------------------------------------------------------------- 1 | #include "einwindow.h" 2 | #include "ui_einwindow.h" 3 | #include "mainwindow.h" 4 | #include "config.h" 5 | 6 | #include 7 | #include 8 | #include 9 | #include "qt_util.h" 10 | 11 | EinWindow::EinWindow(QWidget *parent, MachineState * _ms) : 12 | QMainWindow(parent), 13 | ui(new Ui::EinWindow), 14 | myView(parent, EIN_WINDOW_KEEPRATIO) 15 | { 16 | ui->setupUi(this); 17 | ms = _ms; 18 | ui->imageFrame->layout()->addWidget(myView.getWidget()); 19 | //cout << "menu: " << ui->menubar << endl; 20 | //cout << "menu: " << ui->menubar->isVisible() << endl; 21 | ui->menubar->setVisible(true); 22 | connect(ui->actionSaveImage, SIGNAL(triggered()), this, SLOT(saveImage())); 23 | } 24 | 25 | 26 | void EinWindow::saveImage() { 27 | doSaveImage(ms, this, myImage); 28 | } 29 | 30 | void EinWindow::updateImage(const Mat image) { 31 | if ( !isSketchyMat(image)) { 32 | myView.updateImage(image); 33 | myImage = image; 34 | } 35 | } 36 | 37 | void EinWindow::setMouseCallBack(EinMouseCallback m, void* param) { 38 | myView.setMouseCallBack(m, param); 39 | } 40 | 41 | 42 | void EinWindow::keyPressEvent(QKeyEvent *evnt) { 43 | QWidget::keyPressEvent(evnt); 44 | 45 | } 46 | 47 | 48 | EinWindow::~EinWindow() 49 | { 50 | delete ui; 51 | } 52 | 53 | void EinWindow::toggleVisible(bool show) { 54 | setVisible(show); 55 | } 56 | 57 | void EinWindow::setVisible(bool show) { 58 | QMainWindow::setVisible(show); 59 | emit visibleChanged(show); 60 | } 61 | 62 | -------------------------------------------------------------------------------- /src/qtgui/einwindow.h: -------------------------------------------------------------------------------- 1 | #ifndef EINWINDOW_H 2 | #define EINWINDOW_H 3 | 4 | #include 5 | #include 6 | 7 | #include "window_QT.h" 8 | class MachineState; 9 | 10 | using namespace std; 11 | 12 | 13 | 14 | namespace Ui { 15 | class EinWindow; 16 | } 17 | 18 | class EinWindow : public QMainWindow 19 | { 20 | Q_OBJECT 21 | 22 | public: 23 | explicit EinWindow(QWidget *parent, MachineState * _ms); 24 | ~EinWindow(); 25 | void showImage(CvMat mat); 26 | Q_INVOKABLE void updateImage(const Mat image) ; 27 | Mat myImage; 28 | void setMouseCallBack(EinMouseCallback m, void* param); 29 | void keyPressEvent(QKeyEvent *evnt); 30 | 31 | void setWindowTitle(string s) { 32 | QMainWindow::setWindowTitle(QString::fromStdString(s)); 33 | } 34 | 35 | void setVisible(bool); 36 | 37 | 38 | public slots: 39 | void toggleVisible(bool show); 40 | void saveImage(); 41 | 42 | signals: 43 | void visibleChanged(bool visible); 44 | 45 | private: 46 | Ui::EinWindow *ui; 47 | MachineState * ms; 48 | DefaultEinViewPort myView; 49 | }; 50 | 51 | #endif // EINWINDOW_H 52 | -------------------------------------------------------------------------------- /src/qtgui/einwindow.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | EinWindow 4 | 5 | 6 | 7 | 0 8 | 0 9 | 800 10 | 600 11 | 12 | 13 | 14 | 15 | 0 16 | 0 17 | 18 | 19 | 20 | Ein Window 21 | 22 | 23 | 24 | 25 | 26 | 27 | QFrame::NoFrame 28 | 29 | 30 | QFrame::Raised 31 | 32 | 33 | 0 34 | 35 | 36 | 37 | 0 38 | 39 | 40 | 0 41 | 42 | 43 | 0 44 | 45 | 46 | 0 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 0 57 | 0 58 | 800 59 | 27 60 | 61 | 62 | 63 | 64 | File 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | Save Image 74 | 75 | 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /src/qtgui/gaussianmapwidget.cpp: -------------------------------------------------------------------------------- 1 | #include "gaussianmapwidget.h" 2 | 3 | #include "config.h" 4 | #include "gaussian_map.h" 5 | 6 | 7 | GaussianMapWidget::GaussianMapWidget(QWidget * parent, MachineState * _ms) : QWidget(parent), 8 | ms(_ms), 9 | ui(new Ui::GaussianMapWidget), 10 | meanView(parent, EIN_WINDOW_KEEPRATIO), 11 | stdDevView(parent, EIN_WINDOW_KEEPRATIO), 12 | heightView(parent, EIN_WINDOW_KEEPRATIO) 13 | { 14 | ui->setupUi(this); 15 | //ui->meanImage->layout()->addWidget(meanView.getWidget()); 16 | ui->tabs->widget(0)->layout()->addWidget(meanView.getWidget()); 17 | ui->tabs->widget(1)->layout()->addWidget(stdDevView.getWidget()); 18 | ui->tabs->widget(2)->layout()->addWidget(heightView.getWidget()); 19 | } 20 | 21 | Mat GaussianMapWidget::selectedImage() { 22 | if (ui->tabs->currentIndex() == 0) { 23 | return meanImage; 24 | } else if (ui->tabs->currentIndex() == 1) { 25 | return stdDevImage; 26 | } else if (ui->tabs->currentIndex() == 2) { 27 | return heightImage; 28 | } else { 29 | cout << "Bad index: " << ui->tabs->currentIndex() << endl; 30 | assert(0); 31 | } 32 | 33 | } 34 | void GaussianMapWidget::updateMap(shared_ptr _map) 35 | { 36 | 37 | _map->rgbMuToBgrMat(meanImage); 38 | meanView.updateImage(meanImage); 39 | 40 | _map->rgbSigmaToMat(stdDevImage); 41 | stdDevView.updateImage(stdDevImage); 42 | 43 | 44 | _map->zMuToScaledMat(heightImage); 45 | heightView.updateImage(heightImage); 46 | } 47 | -------------------------------------------------------------------------------- /src/qtgui/gaussianmapwidget.h: -------------------------------------------------------------------------------- 1 | #ifndef GAUSSIANMAPWIDGET_H 2 | #define GAUSSIANMAPWIDGET_H 3 | 4 | #include 5 | class MachineState; 6 | class GaussianMap; 7 | #include "window_QT.h" 8 | #include "stackmodel.h" 9 | 10 | #include "ui_gaussianmapwidget.h" 11 | 12 | #include "capturelineedit.h" 13 | 14 | class GaussianMapWidget : public QWidget 15 | { 16 | Q_OBJECT 17 | 18 | public: 19 | GaussianMapWidget(QWidget * parent, MachineState * ms); 20 | 21 | void updateMap(shared_ptr map); 22 | Mat selectedImage(); 23 | 24 | private: 25 | Ui::GaussianMapWidget *ui; 26 | MachineState * ms; 27 | DefaultEinViewPort meanView; 28 | DefaultEinViewPort stdDevView; 29 | DefaultEinViewPort heightView; 30 | 31 | Mat meanImage; 32 | Mat stdDevImage; 33 | Mat heightImage; 34 | }; 35 | 36 | 37 | #endif // GAUSSIANMAPWIDGET_H 38 | -------------------------------------------------------------------------------- /src/qtgui/gaussianmapwidget.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | GaussianMapWidget 4 | 5 | 6 | 7 | 0 8 | 0 9 | 749 10 | 617 11 | 12 | 13 | 14 | Form 15 | 16 | 17 | 18 | 0 19 | 20 | 21 | 0 22 | 23 | 24 | 0 25 | 26 | 27 | 0 28 | 29 | 30 | 31 | 32 | 0 33 | 34 | 35 | 36 | mean 37 | 38 | 39 | 40 | 41 | 42 | stdDev 43 | 44 | 45 | 46 | 47 | 48 | height 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /src/qtgui/gaussianmapwindow.cpp: -------------------------------------------------------------------------------- 1 | #include "gaussianmapwindow.h" 2 | 3 | #include "ui_gaussianmapwindow.h" 4 | 5 | #include 6 | #include 7 | #include "config.h" 8 | #include "einwindow.h" 9 | #include "qt_util.h" 10 | 11 | GaussianMapWindow::GaussianMapWindow(QWidget *parent, MachineState * _ms) : 12 | QMainWindow(parent), 13 | ui(new Ui::GaussianMapWindow) 14 | { 15 | ui->setupUi(this); 16 | ms = _ms; 17 | widget = new GaussianMapWidget(this, ms); 18 | ui->frame->layout()->addWidget(widget); 19 | ui->menubar->setVisible(true); 20 | connect(ui->actionSaveImage, SIGNAL(triggered()), this, SLOT(saveImage())); 21 | 22 | 23 | } 24 | 25 | void GaussianMapWindow::saveImage() { 26 | doSaveImage(ms, this, widget->selectedImage()); 27 | } 28 | 29 | 30 | GaussianMapWindow::~GaussianMapWindow() 31 | { 32 | delete ui; 33 | delete widget; 34 | } 35 | 36 | 37 | 38 | void GaussianMapWindow::updateMap(shared_ptr map) 39 | { 40 | widget->updateMap(map); 41 | } 42 | 43 | 44 | void GaussianMapWindow::toggleVisible(bool show) { 45 | setVisible(show); 46 | } 47 | 48 | void GaussianMapWindow::setVisible(bool show) { 49 | QMainWindow::setVisible(show); 50 | emit visibleChanged(show); 51 | } 52 | -------------------------------------------------------------------------------- /src/qtgui/gaussianmapwindow.h: -------------------------------------------------------------------------------- 1 | #ifndef GAUSSIANMAPWINDOW_H 2 | #define GAUSSIANMAPWINDOW_H 3 | 4 | #include "gaussianmapwidget.h" 5 | #include 6 | #include 7 | 8 | #include "window_QT.h" 9 | 10 | using namespace std; 11 | 12 | namespace Ui { 13 | class GaussianMapWindow; 14 | } 15 | 16 | class GaussianMapWindow : public QMainWindow 17 | { 18 | Q_OBJECT 19 | 20 | public: 21 | explicit GaussianMapWindow(QWidget *parent, MachineState * _ms); 22 | ~GaussianMapWindow(); 23 | 24 | void setWindowTitle(string s) { 25 | QMainWindow::setWindowTitle(QString::fromStdString(s)); 26 | } 27 | void updateMap(shared_ptr map); 28 | void setVisible(bool); 29 | 30 | 31 | public slots: 32 | void saveImage(); 33 | void toggleVisible(bool show); 34 | 35 | signals: 36 | void visibleChanged(bool visible); 37 | 38 | 39 | private: 40 | Ui::GaussianMapWindow *ui; 41 | MachineState * ms; 42 | GaussianMapWidget *widget; 43 | }; 44 | 45 | #endif // GAUSSIANMAPWINDOW_H 46 | -------------------------------------------------------------------------------- /src/qtgui/gaussianmapwindow.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | GaussianMapWindow 4 | 5 | 6 | 7 | 0 8 | 0 9 | 800 10 | 600 11 | 12 | 13 | 14 | MainWindow 15 | 16 | 17 | 18 | 19 | 20 | 21 | QFrame::NoFrame 22 | 23 | 24 | QFrame::Raised 25 | 26 | 27 | 28 | 0 29 | 30 | 31 | 0 32 | 33 | 34 | 0 35 | 36 | 37 | 0 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 0 48 | 0 49 | 800 50 | 27 51 | 52 | 53 | 54 | 55 | File 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | Save mean image 65 | 66 | 67 | 68 | 69 | Save std dev image 70 | 71 | 72 | 73 | 74 | Save height image 75 | 76 | 77 | 78 | 79 | Save Image 80 | 81 | 82 | 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /src/qtgui/main.cpp: -------------------------------------------------------------------------------- 1 | #include "mainwindow.h" 2 | #include 3 | 4 | int main(int argc, char *argv[]) 5 | { 6 | QApplication a(argc, argv); 7 | MainWindow w; 8 | w.show(); 9 | 10 | return a.exec(); 11 | } 12 | -------------------------------------------------------------------------------- /src/qtgui/mainwindow.cpp: -------------------------------------------------------------------------------- 1 | #include "mainwindow.h" 2 | #include "ui_mainwindow.h" 3 | 4 | #include 5 | 6 | #include "config.h" 7 | #include "windowmanager.h" 8 | #include "wordaction.h" 9 | 10 | 11 | MainWindow::MainWindow(QWidget *parent, MachineState * _right_arm, MachineState * _left_arm) : 12 | QMainWindow(parent), ui(new Ui::MainWindow), rightArmWidget(this, _right_arm), leftArmWidget(this, _left_arm), right_arm(_right_arm), left_arm(_left_arm), 13 | objectMapView(parent, EIN_WINDOW_KEEPRATIO) 14 | { 15 | ui->setupUi(this); 16 | 17 | windowManager.setMenu(ui->menuWindows); 18 | 19 | ui->objectMapViewFrame->layout()->addWidget(objectMapView.getWidget()); 20 | ui->rightArmFrame->layout()->addWidget(&rightArmWidget); 21 | ui->leftArmFrame->layout()->addWidget(&leftArmWidget); 22 | 23 | 24 | if (right_arm != NULL) { 25 | WordAction * rightClearStackAction = new WordAction(ui->menuRightWords, right_arm, name_to_word["clearStack"]); 26 | ui->menuRightWords->addAction(rightClearStackAction); 27 | } 28 | 29 | if (left_arm != NULL) { 30 | WordAction * leftClearStackAction = new WordAction(ui->menuLeftWords, left_arm, name_to_word["clearStack"]); 31 | ui->menuLeftWords->addAction(leftClearStackAction); 32 | } 33 | 34 | 35 | } 36 | 37 | MainWindow::~MainWindow() 38 | { 39 | delete ui; 40 | } 41 | 42 | void MainWindow::update() { 43 | if ( right_arm != NULL && !isSketchyMat(right_arm->config.objectMapViewerImage)) { 44 | objectMapView.updateImage(right_arm->config.objectMapViewerImage); 45 | } else if (left_arm != NULL && !isSketchyMat(left_arm->config.objectMapViewerImage)) { 46 | objectMapView.updateImage(left_arm->config.objectMapViewerImage); 47 | } else { 48 | // no update. 49 | } 50 | } 51 | 52 | 53 | void MainWindow::setObjectMapViewMouseCallBack(EinMouseCallback m, void* param) { 54 | objectMapView.setMouseCallBack(m, param); 55 | } 56 | 57 | 58 | void MainWindow::keyPressEvent(QKeyEvent *event) { 59 | QWidget::keyPressEvent(event); 60 | 61 | } 62 | 63 | 64 | 65 | void MainWindow::addWindow(QMainWindow * window) { 66 | windowManager.addWindow(window); 67 | } 68 | -------------------------------------------------------------------------------- /src/qtgui/mainwindow.h: -------------------------------------------------------------------------------- 1 | #ifndef MAINWINDOW_H 2 | #define MAINWINDOW_H 3 | 4 | #include 5 | class MachineState; 6 | 7 | #include "window_QT.h" 8 | #include "stackmodel.h" 9 | #include "windowmanager.h" 10 | #include "armwidget.h" 11 | 12 | using namespace std; 13 | namespace Ui { 14 | class MainWindow; 15 | } 16 | 17 | 18 | class MainWindow : public QMainWindow 19 | { 20 | Q_OBJECT 21 | 22 | public: 23 | explicit MainWindow(QWidget *parent = 0, MachineState * _right_arm = 0, MachineState * _left_arm = 0); 24 | ~MainWindow(); 25 | void setWristViewMouseCallBack(EinMouseCallback m, void* param); 26 | void setObjectMapViewMouseCallBack(EinMouseCallback m, void* param); 27 | void keyPressEvent(QKeyEvent *evnt); 28 | void addWindow(QMainWindow * window); 29 | void update(); 30 | 31 | private: 32 | Ui::MainWindow *ui; 33 | WindowManager windowManager; 34 | ArmWidget rightArmWidget; 35 | ArmWidget leftArmWidget; 36 | DefaultEinViewPort objectMapView; 37 | MachineState * right_arm; 38 | MachineState * left_arm; 39 | }; 40 | 41 | #endif // MAINWINDOW_H 42 | -------------------------------------------------------------------------------- /src/qtgui/qt_util.cpp: -------------------------------------------------------------------------------- 1 | #include "qt_util.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | using namespace std; 10 | 11 | #include 12 | using namespace boost::algorithm; 13 | 14 | 15 | void doSaveImage(MachineState * ms, QMainWindow * parent, const Mat & image) { 16 | cout << "Saving image." << endl; 17 | QString qFileName = QFileDialog::getSaveFileName(parent, "Save File", 18 | ".", 19 | "Images (*.png *.jpg)"); 20 | string fileName = qFileName.toStdString(); 21 | 22 | cout << "filename: " << fileName << endl; 23 | cout << "channels: " << image.channels() << endl; 24 | cout << "type: " << image.type() << endl; 25 | Mat dst; 26 | if (image.channels() == 1) { 27 | dst = image.clone(); 28 | //dst = dst * 255; 29 | } else { 30 | dst = image; 31 | } 32 | 33 | if (boost::algorithm::ends_with(fileName, "jpg") || 34 | boost::algorithm::ends_with(fileName, "png") || 35 | boost::algorithm::ends_with(fileName, "ppm") || 36 | boost::algorithm::ends_with(fileName, "tif") || 37 | boost::algorithm::ends_with(fileName, "bmp")) { 38 | imwrite(fileName, dst); 39 | } else { 40 | CONSOLE_ERROR(ms, "Bad extension for " << fileName << endl); 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /src/qtgui/qt_util.h: -------------------------------------------------------------------------------- 1 | #ifndef QTUTIL_H 2 | #define QTUTIL_H 3 | 4 | class MachineState; 5 | #include 6 | #include 7 | #include "config.h" 8 | 9 | 10 | void doSaveImage(MachineState * ms, QMainWindow * parent, const Mat & image); 11 | 12 | #endif // QTUTIL_H 13 | -------------------------------------------------------------------------------- /src/qtgui/stackmodel.cpp: -------------------------------------------------------------------------------- 1 | #include "stackmodel.h" 2 | #include "word.h" 3 | 4 | typedef enum { 5 | IDX = 0, 6 | NAME = 1, 7 | DESCRIPTION = 2, 8 | } columns; 9 | 10 | StackModel::StackModel(QObject *parent) 11 | : QAbstractTableModel(parent) 12 | { 13 | } 14 | 15 | void StackModel::setStack(vector > _stack) 16 | { 17 | beginResetModel(); 18 | stack = _stack; 19 | endResetModel(); 20 | } 21 | 22 | 23 | 24 | 25 | int StackModel::rowCount(const QModelIndex & /* parent */) const 26 | { 27 | return stack.size(); 28 | } 29 | 30 | int StackModel::columnCount(const QModelIndex & /* parent */) const 31 | { 32 | return 2; 33 | } 34 | 35 | 36 | QVariant StackModel::data(const QModelIndex &index, int role) const 37 | { 38 | if (!index.isValid() || role != Qt::DisplayRole) { 39 | return QVariant(); 40 | } else { 41 | shared_ptr w = stack[stack.size() - index.row() - 1]; 42 | if (index.column() == IDX) { 43 | return index.row(); 44 | } else if (index.column() == NAME) { 45 | return QString::fromStdString(w->name()); 46 | } else if (index.column() == DESCRIPTION) { 47 | return QString::fromStdString(w->description()); 48 | } else { 49 | assert(0); 50 | } 51 | } 52 | } 53 | 54 | 55 | QVariant StackModel::headerData(int section /* section */, 56 | Qt::Orientation orientation /* orientation */, 57 | int role) const 58 | { 59 | if (orientation == Qt::Vertical) { 60 | return QVariant(); 61 | } 62 | 63 | if (role == Qt::SizeHintRole) { 64 | if (section == IDX) { 65 | return QSize(30, 1); 66 | } else if (section == NAME) { 67 | return QSize(50, 1); 68 | } else if (section == DESCRIPTION) { 69 | return QSize(100, 1); 70 | } else { 71 | cout << "Unknown section: " << section << endl; 72 | assert(0); 73 | } 74 | } else if (role == Qt::DisplayRole) { 75 | if (section == IDX) { 76 | return "Idx"; 77 | } else if (section == NAME) { 78 | return "Name"; 79 | } else if (section == DESCRIPTION) { 80 | return "Description"; 81 | } else { 82 | cout << "Unknown section: " << section << endl; 83 | assert(0); 84 | } 85 | } else { 86 | return QVariant(); 87 | } 88 | } 89 | -------------------------------------------------------------------------------- /src/qtgui/stackmodel.h: -------------------------------------------------------------------------------- 1 | #ifndef STACKMODEL_H 2 | #define STACKMODEL_H 3 | 4 | 5 | #include 6 | #include 7 | 8 | 9 | using namespace std; 10 | 11 | #include 12 | #include 13 | 14 | class Word; 15 | 16 | class StackModel : public QAbstractTableModel 17 | { 18 | Q_OBJECT 19 | 20 | public: 21 | StackModel(QObject *parent = 0); 22 | 23 | void setStack(vector > stack); 24 | 25 | int rowCount(const QModelIndex &parent = QModelIndex()) const Q_DECL_OVERRIDE; 26 | int columnCount(const QModelIndex &parent = QModelIndex()) const Q_DECL_OVERRIDE; 27 | 28 | QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const Q_DECL_OVERRIDE; 29 | QVariant headerData(int section, Qt::Orientation orientation, int role = Qt::DisplayRole) const Q_DECL_OVERRIDE; 30 | 31 | private: 32 | vector > stack; 33 | }; 34 | 35 | 36 | #endif // STACKMODEL_H 37 | -------------------------------------------------------------------------------- /src/qtgui/streamviewerwindow.h: -------------------------------------------------------------------------------- 1 | #ifndef STREAMVIEWERWINDOW_H 2 | #define STREAMVIEWERWINDOW_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "window_QT.h" 9 | class MachineState; 10 | 11 | using namespace std; 12 | 13 | namespace Ui { 14 | class StreamViewerWindow; 15 | } 16 | 17 | class StreamViewerWindow : public QMainWindow 18 | { 19 | Q_OBJECT 20 | 21 | public: 22 | explicit StreamViewerWindow(QWidget *parent, MachineState * _ms); 23 | ~StreamViewerWindow(); 24 | 25 | void setWindowTitle(string s) { 26 | QMainWindow::setWindowTitle(QString::fromStdString(s)); 27 | } 28 | void update(); 29 | void setVisible(bool); 30 | 31 | public slots: 32 | void saveImage(); 33 | void timeValueChanged(int value); 34 | void toggleVisible(bool show); 35 | 36 | signals: 37 | void visibleChanged(bool visible); 38 | 39 | 40 | private: 41 | Ui::StreamViewerWindow *ui; 42 | MachineState * ms; 43 | 44 | DefaultEinViewPort streamImageView; 45 | }; 46 | 47 | #endif // STREAMVIEWERWINDOW_H 48 | -------------------------------------------------------------------------------- /src/qtgui/streamviewerwindow.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | StreamViewerWindow 4 | 5 | 6 | 7 | 0 8 | 0 9 | 800 10 | 600 11 | 12 | 13 | 14 | Stream Viewer 15 | 16 | 17 | 18 | 19 | 20 | 21 | 100 22 | 23 | 24 | Qt::Horizontal 25 | 26 | 27 | 28 | 29 | 30 | 31 | QFrame::NoFrame 32 | 33 | 34 | QFrame::Plain 35 | 36 | 37 | 38 | 0 39 | 40 | 41 | 0 42 | 43 | 44 | 0 45 | 46 | 47 | 0 48 | 49 | 50 | 0 51 | 52 | 53 | 54 | 55 | Current Stream Image: 56 | 57 | 58 | Qt::LinksAccessibleByMouse|Qt::TextSelectableByKeyboard|Qt::TextSelectableByMouse 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 0 71 | 0 72 | 800 73 | 27 74 | 75 | 76 | 77 | 78 | File 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | Save Image 88 | 89 | 90 | 91 | 92 | 93 | 94 | -------------------------------------------------------------------------------- /src/qtgui/untitled.pro: -------------------------------------------------------------------------------- 1 | #------------------------------------------------- 2 | # 3 | # Project created by QtCreator 2015-05-19T14:24:35 4 | # 5 | #------------------------------------------------- 6 | 7 | QT += core gui 8 | 9 | greaterThan(QT_MAJOR_VERSION, 4): QT += widgets 10 | 11 | TARGET = untitled 12 | TEMPLATE = app 13 | 14 | 15 | SOURCES += main.cpp\ 16 | mainwindow.cpp 17 | 18 | HEADERS += mainwindow.h 19 | 20 | FORMS += mainwindow.ui 21 | -------------------------------------------------------------------------------- /src/qtgui/windowmanager.cpp: -------------------------------------------------------------------------------- 1 | #include "windowmanager.h" 2 | 3 | 4 | WindowManager::WindowManager() { 5 | } 6 | 7 | void WindowManager::setMenu(QMenu * _menu) { 8 | menu = _menu; 9 | 10 | } 11 | 12 | void WindowManager::addWindow(QMainWindow * window) { 13 | windows.push_back(window); 14 | QAction * windowAction = new QAction(menu); 15 | windowAction->setText(window->windowTitle()); 16 | windowAction->setCheckable(true); 17 | windowAction->setChecked(window->isVisible()); 18 | 19 | menu->addAction(windowAction); 20 | 21 | windowAction->connect(windowAction, SIGNAL(toggled(bool)), window, SLOT(toggleVisible(bool))); 22 | 23 | 24 | window->connect(window, SIGNAL(visibleChanged(bool)), windowAction, SLOT(setChecked(bool))); 25 | 26 | } 27 | 28 | -------------------------------------------------------------------------------- /src/qtgui/windowmanager.h: -------------------------------------------------------------------------------- 1 | #ifndef WINDOWMANAGER_H 2 | #define WINDOWMANAGER_H 3 | 4 | #include "einwindow.h" 5 | #include 6 | using namespace std; 7 | 8 | class WindowManager { 9 | 10 | public: 11 | WindowManager(); 12 | void addWindow(QMainWindow * window); 13 | 14 | void setMenu(QMenu * menu); 15 | 16 | private: 17 | QMenu * menu; 18 | 19 | vector windows; 20 | 21 | }; 22 | 23 | 24 | #endif // WINDOWMANAGER_H 25 | -------------------------------------------------------------------------------- /src/qtgui/wordaction.cpp: -------------------------------------------------------------------------------- 1 | #include "wordaction.h" 2 | 3 | #include "word.h" 4 | #include "config.h" 5 | 6 | WordAction::WordAction(QMenu * _menu, MachineState * _ms, shared_ptr _word) : QAction(_menu){ 7 | ms = _ms; 8 | word = _word; 9 | setText(QString::fromStdString(word->name())); 10 | connect(this, SIGNAL(triggered()), this, SLOT(execute())); 11 | //connect(this, SIGNAL(triggered()), this, SLOT(pushWord())); 12 | 13 | } 14 | 15 | void WordAction::pushWord() { 16 | ms->pushWord(word); 17 | } 18 | 19 | void WordAction::execute() { 20 | ms->execute(word); 21 | } 22 | 23 | -------------------------------------------------------------------------------- /src/qtgui/wordaction.h: -------------------------------------------------------------------------------- 1 | #ifndef WORDACTION_H 2 | #define WORDACTION_H 3 | 4 | #include 5 | 6 | 7 | #include "einwindow.h" 8 | #include 9 | 10 | using namespace std; 11 | #include 12 | #include 13 | class Word; 14 | class MachineState; 15 | 16 | class WordAction: public QAction { 17 | 18 | Q_OBJECT 19 | 20 | public: 21 | WordAction(QMenu*, MachineState * ms, shared_ptr word); 22 | 23 | public slots: 24 | void pushWord(); 25 | void execute(); 26 | 27 | private: 28 | MachineState * ms; 29 | shared_ptr word; 30 | 31 | }; 32 | 33 | 34 | #endif // WORDACTION_H 35 | -------------------------------------------------------------------------------- /src/slu/math3d.h: -------------------------------------------------------------------------------- 1 | #ifndef MATH3D_H 2 | #define MATH3D_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "gsl_utilities.h" 9 | 10 | 11 | #ifdef __cplusplus 12 | extern "C" { 13 | #endif 14 | 15 | 16 | typedef struct 17 | { 18 | gsl_matrix * points_xy; 19 | double z_start; 20 | double z_end; 21 | int initialized; // should change all args to use pointers, because this doesn't play nice with swig. 22 | } math3d_prism_t; 23 | 24 | math3d_prism_t math3d_prism_init(); 25 | void math3d_prism_free(math3d_prism_t p); 26 | bool math3d_higher_than(math3d_prism_t p1, math3d_prism_t p2); 27 | bool math3d_starts_higher_than(math3d_prism_t p1, math3d_prism_t p2); 28 | bool math3d_supports(math3d_prism_t p1, math3d_prism_t p2); 29 | bool math3d_intersect_prisms(math3d_prism_t p1, math3d_prism_t p2); 30 | gsl_matrix * math3d_compress(gsl_vector * timestamps, 31 | gsl_matrix * points_xyztheta); 32 | 33 | double math3d_dist(gsl_vector* pt1, gsl_vector * pt2); 34 | double math3d_square_dist(gsl_vector* pt1, gsl_vector * pt2); 35 | 36 | 37 | gsl_vector * math3d_intersect_line_plane(gsl_matrix * line_xyz, 38 | gsl_matrix * plane_xyz); 39 | 40 | 41 | #ifdef __cplusplus 42 | } 43 | #endif 44 | 45 | 46 | #endif 47 | 48 | -------------------------------------------------------------------------------- /timecpp.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | 4 | VAR=$((time /usr/bin/clang $*) 2>&1) 5 | 6 | 7 | echo -e "time cmd: \n\n $* \n\n time run: \n\n $VAR done" 8 | 9 | 10 | 11 | 12 | 13 | --------------------------------------------------------------------------------