├── .github └── workflows │ └── cmake.yml ├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── Makefile ├── README.md ├── bin └── bot │ ├── Makefile │ ├── main.cpp │ └── rai.cfg ├── config.mk ├── docs └── howto.md ├── retired ├── 00-rosservice │ ├── Makefile │ └── main.cpp ├── 01-simulator │ ├── Makefile │ └── main.cpp ├── 02-rndPolicy │ ├── MT.cfg │ ├── Makefile │ └── main.cpp ├── 03-planExample │ ├── Makefile │ └── main.cpp ├── 04-fullPickAndPlaceTest │ ├── Makefile │ ├── main.cpp │ └── rai.cfg ├── 05-miniPathToRobot │ ├── Makefile │ └── main.cpp ├── 06-stringService │ ├── Makefile │ └── main.cpp ├── 07-getAndSendToRobot │ ├── Makefile │ ├── main.cpp │ └── rai.cfg ├── 08-ryVersion │ ├── Makefile │ ├── main.cpp │ └── rai.cfg ├── Exec │ ├── Makefile │ ├── execution.cpp │ ├── execution.h │ ├── komo_fine.cpp │ ├── komo_fine.h │ ├── robotio.cpp │ └── robotio.h ├── app │ ├── MT.cfg │ ├── Makefile │ ├── filter.cpp │ ├── filter.h │ ├── fol.g │ ├── help.cpp │ ├── help.h │ ├── main.cpp │ ├── model.g │ ├── problem-01.g │ ├── problem-shared.g │ ├── retired │ │ └── main.cpp │ ├── robot_msgs │ │ ├── SendJointTrajectory.h │ │ ├── SendJointTrajectoryRequest.h │ │ └── SendJointTrajectoryResponse.h │ ├── sim.cpp │ ├── sim.h │ └── solveMain.cxx ├── control │ ├── franka_basic │ │ ├── Makefile │ │ ├── main.cpp │ │ └── rai.cfg │ ├── franka_minimalistic │ │ ├── Makefile │ │ ├── examples_common.cpp │ │ ├── examples_common.h │ │ └── main.cpp │ └── lcmTest │ │ ├── Makefile │ │ └── main.cpp ├── docker │ ├── Dockerfile │ ├── bashrc │ ├── build.sh │ └── run.sh ├── help │ ├── README.md │ ├── bashrc │ ├── docker_build.py │ ├── docker_build_lgp.sh │ ├── docker_run.py │ ├── docker_run_lgp.sh │ ├── install_dependencies_lgp.sh │ ├── spartan.dockerfile │ └── spartanlgp.dockerfile ├── model │ ├── LGP-kuka.g │ ├── check.ipynb │ ├── fol.g │ ├── hook.g │ ├── model.g │ ├── pandaSingle.g │ ├── pandaStation-noTilt.g │ ├── pandaStation-tilted.g │ ├── pandaStation.g │ ├── pandaStation │ │ ├── cameraCalibration.g │ │ ├── meshes │ │ │ ├── .gitignore │ │ │ ├── collision │ │ │ │ ├── finger.stl │ │ │ │ ├── hand.stl │ │ │ │ ├── link0.stl │ │ │ │ ├── link1.stl │ │ │ │ ├── link2.stl │ │ │ │ ├── link3.stl │ │ │ │ ├── link4.stl │ │ │ │ ├── link5.stl │ │ │ │ ├── link6.stl │ │ │ │ └── link7.stl │ │ │ └── visual │ │ │ │ ├── HOWTO.sh │ │ │ │ ├── finger.ply │ │ │ │ ├── hand.ply │ │ │ │ ├── link0.ply │ │ │ │ ├── link1.ply │ │ │ │ ├── link2.ply │ │ │ │ ├── link3.ply │ │ │ │ ├── link4.ply │ │ │ │ ├── link5.ply │ │ │ │ ├── link6.ply │ │ │ │ ├── link7.ply │ │ │ │ └── script.mlx │ │ ├── panda.g │ │ ├── pandaStation.g │ │ ├── panda_clean.g │ │ └── panda_simplerCollisionModels.g │ ├── problem-01.g │ ├── problem-shared.g │ └── stick.g ├── operate │ ├── franka_cmdLine │ │ ├── Makefile │ │ └── main.cpp │ ├── kinematicsTest │ │ ├── Makefile │ │ └── main.cpp │ └── operate │ │ ├── Makefile │ │ └── main.cpp └── perception │ ├── calibObjects │ ├── Makefile │ ├── main.cpp │ └── rai.cfg │ ├── calibration │ ├── Makefile │ └── main.cpp │ ├── cameraCalibration │ ├── Makefile │ ├── cameraCalibration.cpp │ ├── cameraCalibration.h │ ├── cameraCalibrationData_left_X.data │ ├── cameraCalibrationData_left_x.data │ ├── cameraCalibrationData_right_X.data │ ├── cameraCalibrationData_right_x.data │ ├── cameraCalibrationData_simulation_left_X.data │ ├── cameraCalibrationData_simulation_left_x.data │ ├── cameraCalibrationData_simulation_right_X.data │ ├── cameraCalibrationData_simulation_right_x.data │ ├── cameraCalibration_X.data │ ├── cameraCalibration_x.data │ ├── main.cpp │ ├── rai.cfg │ └── world.g │ ├── fullPipeline │ ├── Makefile │ └── main.cpp │ ├── objectDetection │ ├── Makefile │ └── main.cpp │ ├── opencvTests │ ├── Makefile │ └── main.cpp │ ├── realsense │ ├── Makefile │ └── main.cpp │ ├── registration │ ├── Makefile │ ├── main.cpp │ └── rai.cfg │ └── simulateCam │ ├── Makefile │ └── main.cpp ├── src ├── Audio │ ├── Makefile │ ├── audio.cpp │ └── audio.h ├── BotOp │ ├── Makefile │ ├── SecMPC_Stepper.cpp │ ├── SecMPC_Stepper.h │ ├── bot.cpp │ ├── bot.h │ ├── motionHelpers.cpp │ ├── motionHelpers.h │ ├── py-BotOp.cxx │ ├── py-BotOp.h │ ├── simulation.cpp │ ├── simulation.h │ ├── test.ipynb │ ├── tools.cpp │ └── tools.h ├── ConvexDecomposition │ ├── Makefile │ ├── convexDecomp2D.cpp │ └── convexDecomp2D.h ├── FlatVision │ ├── Makefile │ ├── explainBackground.cpp │ ├── explainBackground.h │ ├── explainNovels.cpp │ ├── explainNovels.h │ ├── explainRobot.cpp │ ├── explainRobot.h │ ├── flatVision.cpp │ ├── flatVision.h │ ├── helpers.cpp │ ├── helpers.h │ ├── objectManager.cpp │ ├── objectManager.h │ ├── registrationCalibration.cpp │ ├── registrationCalibration.h │ └── retired │ │ ├── BackgroundSubtractionThread.cpp │ │ ├── BackgroundSubtractionThread.h │ │ ├── _objectManager.cpp │ │ ├── cv_depth_backgroundSubstraction.cpp │ │ ├── cv_depth_backgroundSubstraction.h │ │ ├── explainObject.cpp │ │ └── explainObject.h ├── Franka │ ├── FrankaGripper.cpp │ ├── FrankaGripper.h │ ├── Makefile │ ├── franka.cpp │ ├── franka.h │ ├── help.h │ └── retired │ │ ├── old-franka.cpp │ │ └── old-franka.h ├── IPC │ ├── Makefile │ ├── ipc.cpp │ ├── ipc.h │ ├── ipc.tpp │ └── lcm │ │ ├── Makefile │ │ ├── dbg.h │ │ ├── eventlog.c │ │ ├── eventlog.h │ │ ├── ioutils.h │ │ ├── lcm-cpp-impl.hpp │ │ ├── lcm-cpp.hpp │ │ ├── lcm.c │ │ ├── lcm.h │ │ ├── lcm_coretypes.h │ │ ├── lcm_export.h │ │ ├── lcm_file.c │ │ ├── lcm_internal.h │ │ ├── lcm_memq.c │ │ ├── lcm_mpudpm.c │ │ ├── lcm_tcpq.c │ │ ├── lcm_udpm.c │ │ ├── lcm_version.h │ │ ├── lcmtypes │ │ ├── channel_port_map_update_t.c │ │ ├── channel_port_map_update_t.h │ │ ├── channel_port_mapping.lcm │ │ ├── channel_to_port_t.c │ │ └── channel_to_port_t.h │ │ ├── ringbuffer.c │ │ ├── ringbuffer.h │ │ ├── udpm_util.c │ │ └── udpm_util.h ├── LGPop │ ├── Makefile │ ├── lgpop.cpp │ ├── lgpop.h │ ├── opy.cpp │ └── opy.h ├── MarkerVision │ ├── Makefile │ ├── cvTools.cpp │ ├── cvTools.h │ ├── markerVision.cpp │ ├── markerVision.h │ └── retired │ │ ├── cameraCalibration.cpp │ │ └── cameraCalibration.h ├── NewControl │ ├── Makefile │ ├── TaskControlThread.cpp │ ├── TaskControlThread.h │ ├── ctrlMsgs.h │ ├── taskControl.cpp │ └── taskControl.h ├── Omnibase │ ├── SimplexMotion-com.cpp │ ├── SimplexMotion-com.h │ ├── SimplexMotion.cpp │ ├── SimplexMotion.h │ ├── omnibase.cpp │ └── omnibase.h ├── OptiTrack │ ├── Makefile │ ├── motioncapture.cpp │ ├── motioncapture.h │ ├── motioncapture_optitrack.cpp │ ├── motioncapture_optitrack.h │ ├── optitrack.cpp │ └── optitrack.h ├── RealSense │ ├── Makefile │ ├── MultiRealSenseThread.cpp │ ├── MultiRealSenseThread.h │ ├── RealSenseThread.cpp │ ├── RealSenseThread.h │ └── utils.h ├── Robotiq │ ├── Makefile │ ├── RobotiqGripper.cpp │ └── RobotiqGripper.h ├── Sim │ ├── Makefile │ ├── simulation.cpp │ ├── simulation.h │ ├── simulationIO.cpp │ ├── simulationIO.h │ ├── simulationIO_self.h │ ├── simulator.cpp │ └── simulator.h ├── Utils │ └── timerMacros.h ├── ViveController ├── brokenCodePieces │ ├── drake-internal.h │ ├── drake.cc │ ├── drake.h │ └── sendingAService.cpp └── opencv_reg │ ├── Makefile │ ├── README.md │ ├── map.cpp │ ├── map.hpp │ ├── mapaffine.cpp │ ├── mapaffine.hpp │ ├── mapper.cpp │ ├── mapper.hpp │ ├── mappergradaffine.cpp │ ├── mappergradaffine.hpp │ ├── mappergradeuclid.cpp │ ├── mappergradeuclid.hpp │ ├── mappergradproj.cpp │ ├── mappergradproj.hpp │ ├── mappergradshift.cpp │ ├── mappergradshift.hpp │ ├── mappergradsimilar.cpp │ ├── mappergradsimilar.hpp │ ├── mapperpyramid.cpp │ ├── mapperpyramid.hpp │ ├── mapprojec.cpp │ ├── mapprojec.hpp │ ├── mapshift.cpp │ ├── mapshift.hpp │ └── precomp.hpp └── test ├── 01-openClose ├── Makefile ├── main.cpp └── rai.cfg ├── 02-camera ├── Makefile ├── main.cpp └── rai.cfg ├── 02-multi-camera ├── Makefile ├── main.cpp └── rai.cfg ├── 02-optitrack ├── Makefile ├── main.cpp ├── orgTest.cxx └── rai.cfg ├── 03-blobTracking ├── Makefile ├── main.cpp └── rai.cfg ├── 04-trivialCtrl ├── Makefile ├── main.cpp ├── plt ├── rai.cfg └── retired.cpp ├── 05-moveTo ├── Makefile ├── main.cpp ├── plt └── rai.cfg ├── 06-fastPath ├── Makefile ├── main.cpp ├── plt └── rai.cfg ├── 07-leapCtrl ├── Makefile ├── main.cpp ├── plt └── rai.cfg ├── 08-pnp ├── Makefile ├── main.cpp ├── plt └── rai.cfg ├── 09-rndPoses ├── Makefile ├── main.cpp └── rai.cfg ├── 10-compliant ├── Makefile ├── main.cpp └── rai.cfg ├── 11-calibStation ├── Makefile ├── main.cpp ├── output ├── plt └── rai.cfg ├── 12-controlCalib ├── Makefile ├── main.cpp ├── plt └── rai.cfg ├── 13-reactiveGrasp ├── Makefile ├── droneRace.g ├── graspScenario.g ├── main.cpp ├── plt ├── pushScenario.g └── rai.cfg ├── 14-calibPoints ├── Makefile ├── calib ├── main.cpp ├── rai.cfg └── station.g ├── 20-simulation ├── Makefile ├── main.cpp ├── model.g └── rai.cfg ├── 21-grasp ├── Makefile ├── main.cpp └── rai.cfg ├── py ├── Makefile ├── main.ipynb └── test.py └── retired ├── 02-upDown ├── Makefile └── main.cpp └── 10-calibData ├── Makefile ├── main.cpp └── rai.cfg /.github/workflows/cmake.yml: -------------------------------------------------------------------------------- 1 | name: CMake 2 | 3 | on: [push, pull_request] 4 | 5 | env: 6 | BUILD_TYPE: Release 7 | 8 | jobs: 9 | build: 10 | runs-on: ubuntu-latest 11 | 12 | steps: 13 | - uses: actions/checkout@v2 14 | with: 15 | submodules: 'recursive' 16 | 17 | - name: install ubuntu dependencies 18 | run: | 19 | sudo apt-get install --yes build-essential clang cmake curl git libpoco-dev libeigen3-dev libccd-dev libboost-system-dev portaudio19-dev libglu1-mesa-dev libhidapi-dev 20 | APTGETYES=1 make -C rai -j1 installUbuntuAll 21 | mkdir -p $HOME/git $HOME/.local 22 | 23 | - name: install installer 24 | run: wget https://github.com/MarcToussaint/rai-extern/raw/main/install.sh; chmod a+x install.sh 25 | 26 | - name: install PhysX 27 | run: ./install.sh physx 28 | 29 | - name: install FCL 30 | run: ./install.sh fcl 31 | 32 | - name: install libfranka 33 | run: ./install.sh libfranka 34 | 35 | - name: install python and pybind 36 | run: | 37 | sudo apt install --yes python3 python3-dev python3-pip 38 | echo 'export PATH="${PATH}:$HOME/.local/bin"' >> ~/.bashrc #add this to your .bashrc, if not done already 39 | source ~/.bashrc 40 | python3 -m pip install --user numpy jupyter nbconvert matplotlib pybind11 41 | 42 | - name: setup build files 43 | run: | 44 | export PY_VERSION=`python3 -c "import sys; print(str(sys.version_info[0])+'.'+str(sys.version_info[1]))"` 45 | cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DCMAKE_INSTALL_PREFIX=$HOME/.local -DUSE_PHYSX=ON -DUSE_REALSENSE=OFF -DUSE_OPENCV=OFF -DUSE_OPENCV_LOCAL=OFF -DPY_VERSION=$PY_VERSION -DUSE_QHULL8=ON 46 | 47 | - name: Build 48 | run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} 49 | 50 | - name: install 51 | run: cmake --install ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} 52 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | local.cfg 2 | 3 | unity.cxx 4 | 5 | %% compile temporaries 6 | /build 7 | *.o 8 | *.a 9 | *.so 10 | *.swp 11 | parser.tab.h 12 | parser.tab.cpp 13 | parser.tab.hpp 14 | parser.output 15 | lex.yy.c 16 | *_moc.cpp 17 | *_ui.h 18 | callgrind.out.* 19 | 20 | %% EXECUTABLE 21 | *.exe 22 | 23 | %% IDE temporaries 24 | *.kdev4 25 | vc60.idb 26 | .*.config 27 | .*.creator 28 | .*.creator.user* 29 | .*.files 30 | .*.includes 31 | .*.cflags 32 | .*.cxxflags 33 | 34 | .cproject 35 | .project 36 | *eclipse* 37 | 38 | tags 39 | .cccc 40 | 41 | *.orig 42 | 43 | # PYTHON 44 | *.pyc 45 | _*.py 46 | .ipynb_checkpoints 47 | 48 | # EXECUTION 49 | core 50 | CORE 51 | *.log 52 | 53 | 54 | %% BACKUP/temporary files 55 | *.tmp 56 | *.dat 57 | nogit* 58 | *~ 59 | z.* 60 | z/ 61 | rai.log 62 | .#*# 63 | .#* 64 | #*# 65 | 66 | 67 | %% MAKE 68 | Makefile.dep 69 | Make.lock 70 | .lastMake 71 | makefile 72 | 73 | 74 | %% CMAKE 75 | CMakeCache.txt 76 | CMakeFiles 77 | cmake_install.cmake 78 | CMakeLists.txt.user 79 | CTestTestfile.cmake 80 | 81 | 82 | %% SWIG 83 | *_wrap.cxx 84 | ors.py 85 | orspy.py 86 | 87 | 88 | %% LATEX 89 | *.aux 90 | *.dvi 91 | *.log 92 | *.out 93 | *.toc 94 | *.tdo 95 | *.blg 96 | *.bbl 97 | *.snm 98 | *.nav 99 | *.ilg 100 | *.ind 101 | *.fdb_latexmk 102 | _region_.* 103 | 104 | 105 | %% SPECIAL for git/mlr: 106 | share/gofMake/config.mk 107 | share/extern/tony_mdp/parser.tab.cpp 108 | share/extern/ODE_0.11/download 109 | share/extern/ODE_0.11/ode-0.11 110 | share/extern/ODE_0.11/ode-0.11 111 | share/extern/ode-0.11 112 | share/extern/cuda 113 | share/extern/cudaSDK 114 | share/extern/ntcan 115 | share/extern/opencv-2.1 116 | share/extern/schunkLWA 117 | share/extern/schunkSDH-11-05-11 118 | share/extern/urg-0.0.2 119 | share/extern/esdcan-* 120 | 121 | 122 | %% QUESTIONABLE ???!!! 123 | *.zip 124 | *_doc 125 | *.tgz 126 | 127 | 128 | %% ROS 129 | catkin/ 130 | catkin_generated/ 131 | devel/ 132 | gtest/ 133 | msg_gen 134 | srv_gen 135 | *Action.msg 136 | *ActionFeedback.msg 137 | *ActionGoal.msg 138 | *ActionResult.msg 139 | *Feedback.msg 140 | *Goal.msg 141 | *Result.msg 142 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "rai"] 2 | path = rai 3 | url = ../rai.git 4 | [submodule "rai-robotModels"] 5 | path = rai-robotModels 6 | url = ../rai-robotModels.git 7 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017-2022 Marc Toussaint 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | BASE = rai 2 | BASE2 = . 3 | 4 | DEPEND = KOMO Core Algo Geo Kin Gui Optim Perception 5 | 6 | test_paths = $(shell find test -mindepth 2 -maxdepth 2 -name 'Makefile' -printf "%h ") 7 | 8 | src_paths = $(shell find src -mindepth 1 -maxdepth 1 -type d -not -name 'retired' -printf "%f ") 9 | 10 | 11 | build: $(DEPEND:%=inPath_makeLib/%) 12 | 13 | installUbuntuAll: $(DEPEND:%=inPath_installUbuntu/%) 14 | 15 | printUbuntuAll: $(DEPEND:%=inPath_printUbuntu/%) printUbuntu 16 | 17 | unityAll: $(src_paths:%=inPath_unity/%) 18 | 19 | clean: $(DEPEND:%=inPath_clean/%) 20 | 21 | tests: $(test_paths:%=inPath_make/%) 22 | 23 | runTests: tests 24 | @rm -f z.test-report 25 | @find test -mindepth 1 -maxdepth 1 -type d \ 26 | -exec rai/_make/run-path.sh {} \; 27 | 28 | include $(BASE)/_make/generic.mk 29 | 30 | .NOTPARALLEL: 31 | -------------------------------------------------------------------------------- /bin/bot/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | 4 | DEPEND = Core Algo Gui Geo Kin Franka Control BotOp 5 | #OPENCV = 1 6 | 7 | include $(BASE)/_make/generic.mk 8 | -------------------------------------------------------------------------------- /bin/bot/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | //=========================================================================== 5 | 6 | int main(int argc, char * argv[]){ 7 | rai::initCmdLine(argc, argv); 8 | 9 | //-- setup a configuration 10 | rai::Configuration C; 11 | 12 | if(rai::checkParameter("confFile")){ 13 | C.addFile(rai::getParameter("confFile")); 14 | }else{ 15 | if(rai::getParameter("bot/useArm", "left")=="left"){ 16 | C.addFile(rai::raiPath("../rai-robotModels/scenarios/pandaSingle.g")); 17 | }else{ 18 | C.addFile(rai::raiPath("../rai-robotModels/scenarios/pandasTable-calibrated.g")); 19 | } 20 | } 21 | 22 | BotOp bot(C, !rai::checkParameter("sim")); 23 | 24 | if(rai::checkParameter("close")){ 25 | bot.hold(false, true); 26 | if(bot.gripperL) bot.gripperL->close(); 27 | if(bot.gripperR) bot.gripperR->close(); 28 | if(bot.gripperL) while(!bot.gripperL->isDone()) rai::wait(.1); 29 | if(bot.gripperR) while(!bot.gripperR->isDone()) rai::wait(.1); 30 | } 31 | 32 | if(rai::checkParameter("open")){ 33 | bot.hold(false, true); 34 | if(bot.gripperL) bot.gripperL->open(); 35 | if(bot.gripperR) bot.gripperR->open(); 36 | if(bot.gripperL) while(!bot.gripperL->isDone()) rai::wait(.1); 37 | if(bot.gripperR) while(!bot.gripperR->isDone()) rai::wait(.1); 38 | } 39 | 40 | if(rai::checkParameter("float")){ 41 | bot.hold(true, false); 42 | bot.wait(C, true, false); 43 | } 44 | 45 | if(rai::checkParameter("damp")){ 46 | bot.hold(true, true); 47 | bot.wait(C, true, false); 48 | } 49 | 50 | if(rai::checkParameter("hold")){ 51 | bot.hold(false, true); 52 | bot.wait(C, true, false); 53 | } 54 | 55 | if(rai::checkParameter("up")){ 56 | arr q=bot.qHome; 57 | q(1) -= .5; 58 | if(q.N>7) q(8) -=.5; 59 | bot.moveTo(q, 1.); 60 | bot.wait(C, true, true); 61 | } 62 | 63 | if(rai::checkParameter("loop")){ 64 | arr q=bot.qHome; 65 | bot.moveTo(q, 1.); 66 | bot.wait(C, true, true); 67 | 68 | C.setJointState(bot.qHome); 69 | arr path = getLoopPath(C); 70 | bot.move(path, {5.}); 71 | 72 | bot.wait(C, true, true); 73 | } 74 | 75 | if(rai::checkParameter("home")){ 76 | arr q=bot.qHome; 77 | bot.moveTo(q, 1.); 78 | bot.wait(C, true, true); 79 | } 80 | 81 | cout <<"bye bye" < 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | //====================================================================== 14 | 15 | bool echo(rai_msgs::StringStringRequest &req, 16 | rai_msgs::StringStringResponse &res){ 17 | res.str = req.str; 18 | return true; 19 | } 20 | 21 | //====================================================================== 22 | 23 | int main(int argc,char **argv){ 24 | ros::init(argc, argv, "echo_service"); 25 | 26 | ros::NodeHandle n; 27 | 28 | ros::ServiceServer service = n.advertiseService("echo", echo); 29 | ROS_INFO("Ready to echo."); 30 | ros::spin(); 31 | 32 | return 0; 33 | } 34 | 35 | //=========================================================================== 36 | 37 | /**int main(int argc, char** argv) { 38 | setLogLevels(0,0); 39 | rai::initCmdLine(argc, argv); 40 | ros::init(rai::argc, rai::argv, "RAPshell"); 41 | 42 | if(argc<2) return query("getState"); 43 | if(!strcmp(argv[1],"st")) return query("getState"); 44 | if(!strcmp(argv[1],"sy")) return query("getSymbols"); 45 | 46 | //-- send a fact 47 | rai::String fact; 48 | fact <<"( "; 49 | for(int i=1;i 2 | #include 3 | 4 | //============================================================================= 5 | 6 | int main(int argc,char **argv){ 7 | rai::initCmdLine(argc, argv); 8 | 9 | #if 0 10 | Simulator sim("../model/LGP-kuka.g"); 11 | #else 12 | SimulationIO sim("../model/LGP-kuka.g"); 13 | #endif 14 | 15 | sim.loop(); 16 | 17 | return 0; 18 | } 19 | -------------------------------------------------------------------------------- /retired/02-rndPolicy/MT.cfg: -------------------------------------------------------------------------------- 1 | opt/ParticleAroundWalls/T = 1000 2 | opt/ParticleAroundWalls/k = 2 3 | opt/ParticleAroundWalls/n = 3 4 | opt/ParticleAroundWalls/hardConstrained = 0 5 | 6 | Hrate = .2 7 | 8 | opt/constrainedMethod = 5 2 9 | #opt/verbose=2 10 | #opt/damping=1e-2 11 | #opt/dampingDec=1. 12 | #opt/dampingInc=1. 13 | #opt/stepDec=.2 14 | #opt/stepInc=1.5 15 | #opt/maxStep=1e-1 16 | #opt/nonStrictSteps=-1 17 | #opt/stopTolerance=1e-3 18 | #opt/stopGTolerance=1e-2 19 | 20 | #opt/stopIters = 50 21 | 22 | #opt/stopOuters = 5 23 | KOMO/verbose = 1 24 | -------------------------------------------------------------------------------- /retired/02-rndPolicy/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../rai 2 | 3 | DEPEND = Core Kin Gui Geo KOMO Optim RosCom 4 | 5 | include $(BASE)/_make/generic.mk 6 | -------------------------------------------------------------------------------- /retired/02-rndPolicy/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | 13 | //============================================================================= 14 | 15 | void rndPolicy(){ 16 | RosCom ROS("rndPolicy"); 17 | 18 | rai::KinematicWorld K("../model/LGP-kuka.g"); 19 | arr q0 = K.getJointState(); 20 | StringA joints = K.getJointNames(); 21 | cout <<"JOINTS: " < ref("MotionReference"); 26 | 27 | auto pubRef = ROS.publish(ref); 28 | 29 | for(uint i=0;;i++){ 30 | rai::wait(1e-3*rnd(10,1000)/speed); 31 | 32 | uint T = rnd(1,10); 33 | arr x = repmat(~q0,T,1); 34 | rndUniform(x, -3., 3., true); 35 | 36 | arr t = zeros(T); 37 | rndUniform(t, .5/speed, 2./speed); 38 | 39 | ref.writeAccess(); 40 | ref->x = conv_arr2arr(x); 41 | ref->t = conv_arr2arr(t); 42 | ref->append = rnd(0,1); 43 | ref->revision = i; 44 | ref.deAccess(); //publishes automatically 45 | } 46 | } 47 | 48 | //============================================================================= 49 | 50 | int main(int argc,char **argv){ 51 | rai::initCmdLine(argc, argv); 52 | 53 | rndPolicy(); 54 | 55 | return 0; 56 | } 57 | -------------------------------------------------------------------------------- /retired/03-planExample/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../rai 2 | 3 | DEPEND = Core Kin Gui Geo KOMO Optim RosCom 4 | 5 | include $(BASE)/_make/generic.mk 6 | -------------------------------------------------------------------------------- /retired/03-planExample/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | //====================================================================== 5 | 6 | void optSkeleton(const Skeleton& S, const rai::KinematicWorld& K){ 7 | double maxPhase = 1.; 8 | for(const SkeletonEntry& s:S){ 9 | if(s.phase0 > maxPhase) maxPhase = s.phase0; 10 | if(s.phase1 > maxPhase) maxPhase = s.phase1; 11 | } 12 | maxPhase += .5; 13 | 14 | KOMO komo; 15 | komo.setModel(K, true); 16 | komo.setPathOpt(maxPhase, 10., 2.); 17 | komo.setCollisions(false); 18 | komo.deactivateCollisions("table1", "iiwa_link_0_0"); 19 | komo.activateCollisions("table1", "stick"); 20 | komo.activateCollisions("table1", "stickTip"); 21 | 22 | komo.setSkeleton(S); 23 | 24 | komo.reset(); 25 | // komo.reportProblem(); 26 | komo.animateOptimization = 0; 27 | komo.run(); 28 | // komo.checkGradients(); 29 | 30 | cout < 2 | #include 3 | 4 | struct StringService { 5 | ros::NodeHandle nh; 6 | ros::ServiceServer service; 7 | bool cb_service(rai_msgs::StringString::Request& _request, rai_msgs::StringString::Response& _response); 8 | 9 | StringService(); 10 | ~StringService(){} 11 | }; 12 | 13 | 14 | StringService::StringService() { 15 | LOG(1) <<"*** Starting String Service" < 2 | #include 3 | 4 | #include 5 | 6 | //====================================================================== 7 | 8 | int main(int argc,char **argv){ 9 | rai::initCmdLine(argc, argv); 10 | rnd.clockSeed(); 11 | 12 | ry::Configuration K; 13 | K.addFile("../model/model.g"); 14 | 15 | auto D = K.camera(); 16 | 17 | 18 | 19 | 20 | LGPExecution E(false, true, ); 21 | 22 | //-- compute the rough full skeleton plan 23 | Skeleton S = { 24 | // { {"grasp", "endeff", "stick"}, 1., 1.}, 25 | // { {"touch", "stick", "redBall"}, 2., 2.}, 26 | // { {"place", "stick", "table1"}, 2., 2.}, 27 | { {"grasp", "endeff", "stick"}, 1., 1.}, 28 | { {"grasp", "stickTip", "redBall"}, 2., 2.}, 29 | }; 30 | ptr plan = E.planSkeleton(S); 31 | 32 | //-- extract the relevant key pose 33 | arr q_grasp_rough = E.getJointConfiguration(plan, 1.); 34 | 35 | //-- compute the precise key pose 36 | arr q_grasp_precise = E.computePreciseGrasp(q_grasp_rough, "stick"); 37 | 38 | //-- compute first motion, execute, close gripper 39 | E.syncModelJointStateWithRealOrSimulation(); 40 | auto q_grasp_motion = E.computePreciseMotion(q_grasp_precise, false, true); 41 | E.executeMotion(E.armJoints, q_grasp_motion.first, q_grasp_motion.second); 42 | E.closeGripper(); 43 | 44 | //-- wait, send 'attach' 45 | E.waitForCompletion(); 46 | E.attach("endeff", "stick"); 47 | 48 | //-- compute second motion, execute, open gripper 49 | arr q_place_rough = E.getJointConfiguration(plan, 2.); 50 | rai::Transformation X = E.getObjectPose(plan, 2., "stick"); 51 | arr q_place_precise = E.computePrecisePlace(q_place_rough, "stick", "table1", X); 52 | E.syncModelJointStateWithRealOrSimulation(); 53 | auto q_place_motion = E.computePreciseMotion(q_place_precise, true, true); 54 | E.executeMotion(E.armJoints, q_place_motion.first, q_place_motion.second); 55 | E.openGripper(); 56 | 57 | //-- wait, send 'attach' 58 | E.waitForCompletion(); 59 | E.attach("table1", "stick"); 60 | 61 | //-- compute homing motion, execute 62 | E.syncModelJointStateWithRealOrSimulation(); 63 | auto q_homing = E.computePreciseMotion(E.q_home, true, false); 64 | E.executeMotion(E.armJoints, q_homing.first, q_homing.second); 65 | 66 | //-- wait 67 | E.waitForCompletion(); 68 | rai::wait(); 69 | return 0; 70 | } 71 | 72 | -------------------------------------------------------------------------------- /retired/08-ryVersion/rai.cfg: -------------------------------------------------------------------------------- 1 | useRos = 0 2 | guiPauses = 0 3 | -------------------------------------------------------------------------------- /retired/Exec/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | NAME = $(shell basename `pwd`) 4 | OUTPUT = lib$(NAME).so 5 | 6 | CPATHS += $(HOME)/spartan/build/install/include 7 | 8 | DEPEND = Sim Core Kin Gui Geo KOMO Optim RosCom Algo 9 | 10 | ROS = 1 11 | 12 | SRCS = $(shell find . -maxdepth 1 -name '*.cpp' ) 13 | OBJS = $(SRCS:%.cpp=%.o) 14 | 15 | include $(BASE)/_make/generic.mk 16 | -------------------------------------------------------------------------------- /retired/Exec/execution.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "robotio.h" 7 | 8 | /** This is a high helper for LGP execution 9 | * 10 | * this code is not generic, but very specific to our LGP execution experiments 11 | */ 12 | 13 | template using ptr = std::shared_ptr; 14 | 15 | struct LGPExecution{ 16 | SimulationIO S; 17 | RobotIO R; 18 | rai::KinematicWorld K; 19 | StringA allJoints, armJoints, gripperJoints; 20 | arr q_home; 21 | bool guiPauses = true; 22 | bool useRealRobot=false; 23 | bool useSimulation=false; 24 | double timeScale=1.; 25 | 26 | // the object poses are initially set to unknown/random 27 | LGPExecution(bool _useRealRobot, bool _useSimulation, const char* modelFile = "../model/model.g"); 28 | 29 | void startSimulation(); 30 | void stopSimulation(); 31 | 32 | //-- sync the model's state with the real (or sim) state 33 | void syncModelJointStateWithRealOrSimulation(); 34 | void syncModelObjectPosesWithRealOrSimulation(uint average); 35 | 36 | void setModelJointState(const StringA& joints, const arr& q); 37 | 38 | //-- planning 39 | 40 | // We plan a full pick-and-place of one object using LGP (1st use of KOMO) 41 | ptr planSkeleton(const Skeleton& S); 42 | 43 | // We retrieve the key frame poses 44 | arr getJointConfiguration(ptr plan, double phase); 45 | rai::Transformation getObjectPose(ptr plan, double phase, const char* obj); 46 | 47 | // Use 1. keyframe as regularization for a high-precision grasp pose IK (2nd use of KOMO) 48 | arr computePreciseGrasp(const arr& roughPose, const char* obj); 49 | 50 | // Use the 2. keyframe as regularization for a high-prediction placement IK 51 | arr computePrecisePlace(const arr& roughPose, const char* obj, const char* onto, const rai::Transformation& Q); 52 | 53 | // For this grasp pose, generate a nice motion with final downward motion (3nd use of KOMO) 54 | std::pair computePreciseMotion(const arr& to, bool initialUp, bool finalDown); 55 | 56 | //-- validation 57 | void displayAndValidate(const StringA& joints, const arr& q, const arr& tau); 58 | 59 | //-- execution 60 | void executeMotion(const StringA& joints, const arr& q, const arr& tau); 61 | void waitForCompletion(); 62 | void closeGripper(); 63 | void openGripper(); 64 | void attach(const char* a, const char* b); 65 | void homing(bool initialUp); 66 | }; 67 | -------------------------------------------------------------------------------- /retired/Exec/komo_fine.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | //=============================================================================== 4 | 5 | struct KOMO_fineManip : KOMO{ 6 | KOMO_fineManip(const rai::KinematicWorld& K) : KOMO(K){} 7 | 8 | void setFineGrasp(double time, const char *endeff, const char *object, const char* gripper); 9 | void setFinePlace(double time, const char *endeff, const char *object, const char* placeRef, const char* gripper); 10 | void setFinePlaceFixed(double time, const char *endeff, const char *object, const char *placeRef, const rai::Transformation& worldPose, const char* gripper); 11 | 12 | void setFineLift(double time, const char *endeff); 13 | void setFineHoming(double time, const char *gripper); 14 | 15 | void setIKGrasp(const char *endeff, const char *object); 16 | void setIKPlace(const char *object, const char* onto, const rai::Transformation &worldPose); 17 | void setIKPlaceFixed(const char *object, const rai::Transformation& worldPose); 18 | void setIKPush(const char* stick, const char* object, const char* table); 19 | 20 | void setGoTo(const arr& q, const StringA& joints, const char *endeff, double up=.2, double down=.8); 21 | 22 | void setIKAbstractTask(const StringA &facts, int verbose=0); 23 | }; 24 | -------------------------------------------------------------------------------- /retired/Exec/robotio.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "robotio.h" 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | struct RobotIO{ 10 | RosCom ROS; 11 | bool pubSubToROS; 12 | 13 | StringA jointNames; 14 | 15 | Var jointState; 16 | Var tfMessages; 17 | Var gripperCommand; 18 | std::map*> objectStates; 19 | 20 | 21 | std::shared_ptr> sub_jointState; //subscriber 22 | std::shared_ptr> sub_tfMessages; //subscriber 23 | std::shared_ptr> pub_gripperCommand; //subscriber 24 | std::map>> sub_objectStates; //subscribers 25 | 26 | uint gripperCommandCounter=0; 27 | 28 | RobotIO(bool pubSubToROS); 29 | 30 | //-- execution 31 | bool executeMotion(const StringA& joints, const arr& path, const arr& times, double timeScale=1.); 32 | void execGripper(double position, double force=40.); 33 | 34 | arr getJointPositions(const StringA& joints); 35 | 36 | //see example code in rai/RosCom/perc/subscribeOptitrack.h 37 | arr getObjectPoses(const StringA& objs); 38 | 39 | StringA getJointNames(); 40 | 41 | StringA getObjectNames(); 42 | }; 43 | 44 | -------------------------------------------------------------------------------- /retired/app/MT.cfg: -------------------------------------------------------------------------------- 1 | useRos = 1 2 | robot = pr2 3 | #model.g 4 | oldfashinedTaskControl = 1 5 | fixBase = 1 6 | useSwift = 1 7 | hyperSpeed = 1. 8 | 9 | taskControllerNoUseRos = 1 10 | 11 | rosNodeName = marcs_node 12 | #Hrate = 1. 13 | #kinectDepthPixelShift_x = 7 14 | #kinectDepthPixelShift_y = -2 15 | #kinectColorBGRSwap = 1 16 | kinectFrameShift = [-.015 -.005 0] 17 | 18 | controller_kp_factor = 1. 19 | controller_kd_factor = 1.5 20 | controller_ki_factor = .2 21 | 22 | #opt/muInit = 10. 23 | #opt/stepInc = 1.5 24 | #opt/stepDec = .2 25 | opt/verbose = 0 1 26 | #KOMO/verbose = 1 27 | 28 | #opt/constrainedMethod = 2 29 | -------------------------------------------------------------------------------- /retired/app/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../rai 2 | 3 | SRCS = $(shell find . -maxdepth 1 -name '*.cpp' ) 4 | OBJS = $(SRCS:%.cpp=%.o) 5 | 6 | GL = 1 7 | ROS = 0 8 | DEPEND = Core Gui Kin KOMO Geo Algo RosCom Perception Control LGP Logic 9 | 10 | include $(BASE)/_make/generic.mk 11 | -------------------------------------------------------------------------------- /retired/app/filter.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | //=============================================================================== 6 | 7 | struct PerceptSimple; 8 | typedef rai::Array PerceptSimpleL; 9 | 10 | struct PerceptSimple : GLDrawer{ 11 | enum Type { PT_cluster, PT_plane, PT_box, PT_mesh, PT_alvar, PT_optitrackmarker, PT_optitrackbody, PT_end }; 12 | 13 | rai::GeomStore& store; 14 | int geomID = -1; 15 | int frameID = -1; 16 | rai::Transformation pose; 17 | double precision = 1.; //1 is maximum; decays as a precision in kalman filtering 18 | PerceptSimple() : store(_GeomStore()), pose(0) {} 19 | PerceptSimple(const rai::Geom& geom, const rai::Transformation& pose) 20 | : store(_GeomStore()), geomID(geom.ID), pose(pose) { 21 | } 22 | virtual ~PerceptSimple(){} 23 | 24 | virtual double fuse(PerceptSimple* other, double alpha=.2); 25 | virtual void write(ostream& os) const; 26 | virtual void glDraw(OpenGL& gl); 27 | virtual PerceptSimple* newClone() const{ return new PerceptSimple(*this); } 28 | 29 | }; 30 | stdOutPipe(PerceptSimple) 31 | 32 | //=============================================================================== 33 | 34 | struct FilterSimple : Thread{ 35 | rai::KinematicWorld K; 36 | FrameL objects; 37 | PerceptSimpleL percepts_display; 38 | double time=0.; 39 | 40 | Var percepts_input; //perception sensors 41 | Var percepts_filtered; 42 | Var currentQ; //joint/gripper/base sensors 43 | Var currentGripper; 44 | Var robotBase; 45 | 46 | Var switches; //receive kinSwitch command from policy 47 | Var timeToGo; // 48 | Var filterWorld; //publish the filtered world? 49 | Var problemPerceived = {this, "problemPerceived"}; 50 | 51 | FilterSimple(double dt=.01); 52 | ~FilterSimple(){ 53 | } 54 | 55 | void open(){} 56 | void close(){} 57 | 58 | void step(); 59 | }; 60 | 61 | -------------------------------------------------------------------------------- /retired/app/help.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | void callTrajectoryService(const Msg_MotionReference& msg); 5 | 6 | Msg_MotionReference planPath_IK(const StringA& cmd, const rai::Transformation& where=NoTransformation, bool fromCurrent=true); 7 | 8 | void solveLGP(); 9 | -------------------------------------------------------------------------------- /retired/app/main.cpp: -------------------------------------------------------------------------------- 1 | #include "sim.h" 2 | #include "komo_fine.h" 3 | #include "filter.h" 4 | #include "help.h" 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | //=============================================================================== 11 | 12 | void execAction(const StringA& action, const rai::Transformation& rel=NoTransformation){ 13 | Msg_MotionReference ref = planPath_IK(action, rel); 14 | Var("MotionReference").set() = ref; 15 | Var ttg("timeToGo"); 16 | callTrajectoryService(ref); 17 | rai::wait(1.); 18 | for(;;){ ttg.waitForNextRevision(); if(ttg.get()<=0.) break; } 19 | } 20 | 21 | void execGripper(double q, const StringA& kinSwitch={}){ 22 | Var("gripperReference").set() = q; 23 | if(kinSwitch.N) Var("switches").set() = kinSwitch; 24 | rai::wait(.3); 25 | } 26 | 27 | void go(int probId=1) { 28 | //-- load the problem description into the 'world' variable 29 | Var("world").set()->init(STRING("problem-0"<("world").get()->getJointNames(); 33 | joints.removeValue("slider1Joint", false); 34 | joints.removeValue("wsg_50_base_joint_gripper_left"); 35 | Var("jointNames").set() = joints; 36 | 37 | //-- start a simulation 38 | KinSim sim; 39 | sim.threadLoop(); 40 | 41 | //-- start the filter 42 | FilterSimple filter; 43 | filter.threadLoop(); 44 | 45 | rai::wait(.1); 46 | 47 | { 48 | Var q("currentQ"); 49 | Var K("world"); 50 | if(maxDiff(q.get()(), K.get()->getJointState(joints)) > 1e-2){ 51 | execAction({"home"}); 52 | } 53 | } 54 | 55 | //wait for all objects to be localized 56 | filter.problemPerceived.waitForValueEq(true); 57 | 58 | // solveLGP(); 59 | // return; 60 | 61 | for(uint l=0;;l++){ 62 | execAction({"grasp", "endeff", "stick"}); 63 | 64 | execGripper(.004, {"attach", "endeff", "nostick"}); 65 | 66 | execAction({"push","stickTip","redBall","table1"}, { {.5, -.5, 1.12}, Quaternion_Id } ); 67 | 68 | execGripper(.05, {"attach", "table1", "nostick"}); 69 | } 70 | 71 | 72 | } 73 | 74 | //=============================================================================== 75 | 76 | int main(int argc, char** argv){ 77 | rai::initCmdLine(argc, argv); 78 | 79 | RosCom_Spinner rs; 80 | rs.threadLoop(); 81 | 82 | go(); 83 | 84 | return 0; 85 | } 86 | 87 | -------------------------------------------------------------------------------- /retired/app/model.g: -------------------------------------------------------------------------------- 1 | Include='problem-01.g' 2 | 3 | frame box0 { shape=ssBox size=[.05 .05 .16 .01] color=[.7 .1 .1] X= percept } 4 | -------------------------------------------------------------------------------- /retired/app/problem-01.g: -------------------------------------------------------------------------------- 1 | Include = 'problem-shared.g' 2 | 3 | ### ball 4 | 5 | body redBall { size=[.06 .06 .06 .02] color=[1 0 0] type=ssBox, contact, percept, logical={ object } } 6 | joint (table1 redBall) { from= type=rigid } 7 | 8 | ### hook 9 | 10 | body nostick { type=5 size=[.2 .2 .2] } 11 | joint (table1 nostick) { from= type=rigid} 12 | shape stick(nostick) { type=9 size=[.8 .025 .04 .01] color=[.6 .3 0] contact, percept, logical={ object } } 13 | shape stickTip (nostick) { rel= type=9 size=[.2 .026 .04 0.01] color=[.6 .3 0], logical={ object, pusher } } 14 | 15 | -------------------------------------------------------------------------------- /retired/app/problem-shared.g: -------------------------------------------------------------------------------- 1 | #Include = '../../data/baxter_model/baxter.ors' 2 | Include='../rai-robotModels/kuka_drake/setup.g' 3 | 4 | Edit base { X= } # for kuka 5 | #Edit base_footprint { X= } # for baxter 6 | 7 | body slider1a { type=box size=[.2 .02 .02 0] color=[.5 .5 .5] } 8 | body slider1b { type=box size=[.2 .02 .02 0] color=[.8 .3 .3] } 9 | joint slider1Joint(slider1a slider1b){ type=transX } 10 | #shape (slider1b){ rel= type=5 size=[.1 .1 .1] color=[0 1 0] } 11 | 12 | body table1{ type=9, X=, size=[2. 3. .04 .02], color=[.3 .3 .3] fixed, contact, logical={ table } } 13 | 14 | Edit endeff { logical={ gripper, free } } 15 | -------------------------------------------------------------------------------- /retired/app/sim.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | //=============================================================================== 7 | 8 | struct PerceptSimple; 9 | typedef rai::Array PerceptSimpleL; 10 | 11 | //=============================================================================== 12 | 13 | struct KinSim : Thread{ 14 | rai::KinematicWorld K; 15 | uint pathRev=0, switchesRev=0; 16 | rai::Spline reference; 17 | double phase=0.; 18 | double dt, planDuration=5.; 19 | ofstream log; 20 | 21 | //input 22 | Var ref; 23 | Var switches; 24 | Var gripper; 25 | 26 | //output 27 | Var currentQ; 28 | Var currentGripper; 29 | Var jointNames; 30 | Var robotBase; 31 | // Var nextQ; 32 | // Var world; 33 | Var timeToGo; 34 | Var percepts_input; 35 | 36 | 37 | KinSim(double dt=.1); 38 | ~KinSim(){ 39 | log.close(); 40 | } 41 | 42 | void open(){} 43 | void close(){} 44 | 45 | void step(); 46 | 47 | }; 48 | -------------------------------------------------------------------------------- /retired/control/franka_basic/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | 4 | DEPEND = Core Gui Franka Kin 5 | 6 | include $(BASE)/_make/generic.mk 7 | -------------------------------------------------------------------------------- /retired/control/franka_basic/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | int main(int argc, char** argv) { 6 | /* minimal demo to hold robot in current state - with set gains */ 7 | 8 | Var ctrl; 9 | Var state; 10 | 11 | FrankaThread F(ctrl, state, 1); 12 | 13 | //control robot to current state -- this is automatically done in the fanka launch 14 | // ctrl.set()->q = state.get()->q; 15 | 16 | rai::KinematicWorld K("../../model/pandaSingle.g"); 17 | 18 | for(;;){ 19 | rai::wait(.02); 20 | arr q = state.get()->q; 21 | q.append(.1); 22 | K.setJointState(q); 23 | if(K.watch()=='q') break; 24 | } 25 | 26 | return 0; 27 | } 28 | -------------------------------------------------------------------------------- /retired/control/franka_basic/rai.cfg: -------------------------------------------------------------------------------- 1 | Franka/Kp_freq: [15., 15., 15., 10., 5., 5., 3.] 2 | Franka/Kd_ratio: [.8, .8, .7, .7, .1, .1, .1] 3 | -------------------------------------------------------------------------------- /retired/control/franka_minimalistic/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | 3 | DEPEND = Core Gui 4 | 5 | OBJS = main.o examples_common.o 6 | 7 | EIGEN=1 8 | 9 | CPATH := $(CPATH):$(HOME)/git/libfranka/include 10 | LPATH := $(LPATH):$(HOME)/git/libfranka/build 11 | LIBS += -lfranka 12 | 13 | include $(BASE)/_make/generic.mk 14 | -------------------------------------------------------------------------------- /retired/control/lcmTest/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | 4 | DEPEND = Core IPC 5 | 6 | include $(BASE)/_make/generic.mk 7 | -------------------------------------------------------------------------------- /retired/control/lcmTest/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | 6 | struct SomeMsg : CompoundSerializable{ 7 | arr x; 8 | arr y; 9 | SomeMsg() : CompoundSerializable({&x, &y}) {} 10 | }; 11 | 12 | void sender(){ 13 | Var x; 14 | lcm_publish(x, "X"); 15 | 16 | for(uint k=0;k<10;k++){ 17 | if(rnd.uni()<.5) 18 | x.set()->x = consts((double)k, k); 19 | else 20 | x.set()->y = consts((double)k, k, k/2); 21 | 22 | cout <<"published: x=" <x <<" y=" <y < x; 30 | lcm_subscribe(x, "X"); 31 | 32 | while(true){ 33 | x.waitForNextRevision(); 34 | cout <<"received: x=" <x <<" y=" <y < /etc/apt/sources.list.d/ros-latest.list' 10 | RUN apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116 11 | RUN apt-get --yes update 12 | 13 | # install full dependencies 14 | RUN apt-get --yes install \ 15 | g++ liblapack-dev libf2c2-dev gnuplot libjsoncpp-dev libx11-dev \ 16 | libx11-dev libpng12-dev freeglut3-dev libglew-dev graphviz-dev \ 17 | libann-dev \ 18 | libassimp-dev libglew-dev libqhull-dev \ 19 | ros-kinetic-desktop ros-kinetic-object-recognition-msgs \ 20 | ros-kinetic-ar-track-alvar-msgs \ 21 | libpcl-dev 22 | 23 | RUN apt-get --yes install \ 24 | ros-kinetic-vrpn-client-ros 25 | 26 | WORKDIR /root 27 | 28 | RUN echo 'source ~/bashrc' >> ~/.bashrc -------------------------------------------------------------------------------- /retired/docker/bashrc: -------------------------------------------------------------------------------- 1 | export PS1='DOCKER \u@\h:\w> ' 2 | 3 | export MAKEFLAGS="-j4 -k" 4 | 5 | export ROSDIR=/opt/ros/kinetic 6 | source ${ROSDIR}/setup.bash 7 | 8 | alias ls="ls -F --color" 9 | alias ll="ls -lFh --color" 10 | alias la="ls -alFh --color" 11 | alias lt="ls -rtlFh --color" 12 | alias lS="ls -rSlFh --color" 13 | alias cp="cp -ipvP" 14 | alias mv="mv -iv" 15 | alias rm="rm -iv" 16 | 17 | alias qtcreator='$HOME/local/opt/qtcreator-4.6.1/bin/qtcreator' 18 | -------------------------------------------------------------------------------- /retired/docker/build.sh: -------------------------------------------------------------------------------- 1 | #/bin/sh 2 | 3 | docker build --tag lgp-exec --network host . 4 | -------------------------------------------------------------------------------- /retired/docker/run.sh: -------------------------------------------------------------------------------- 1 | #/bin/sh 2 | 3 | thispath=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd) 4 | 5 | xhost +local:root 6 | 7 | nvidia-docker run -it \ 8 | --volume="$thispath/../..:/root/local" \ 9 | --volume="$thispath/bashrc:/root/bashrc" \ 10 | --volume="$HOME/.gitconfig:/root/.gitconfig:ro" \ 11 | --volume="$HOME/.ssh:/root/.ssh:ro" \ 12 | --env="DISPLAY" \ 13 | --publish 11311:11311 \ 14 | --network host \ 15 | lgp-exec /bin/bash 16 | 17 | xhost -local:root 18 | -------------------------------------------------------------------------------- /retired/help/README.md: -------------------------------------------------------------------------------- 1 | TODOs: 2 | * get the whole thing going (in simulation, as it was on your mac) on the iwaa computer 3 | * implement the robotio.cpp 4 | 5 | StartHere: 6 | https://github.com/RobotLocomotion/KukaHardware 7 | More on getting spartan running in docker: 8 | https://github.com/RobotLocomotion/spartan/blob/master/setup/docker/README.md 9 | 10 | https://github.com/RobotLocomotion/KukaHardware/blob/master/robot_operation_instructions.md 11 | 12 | Possible needed changes in `docker_build.py`: 13 | ``` 14 | cmd += " --network=host" 15 | cmd += " --no-cache" 16 | ``` 17 | 18 | changes in `docker_run.py`: 19 | ``` 20 | cmd += " -v ~/local:%(home_directory)s/local " % {'home_directory': home_directory} # mount local path into docker 21 | 22 | cmd += " -p 11311 " # expose ROS ports 23 | cmd += " -p 3883 " # expose VRPN ports 24 | cmd += "--network host" 25 | ``` 26 | 27 | when compiling in the docker, in ccmake: 28 | ``` 29 | BULLET3: ON 30 | RLG: ON 31 | TRI: OFF 32 | SNOPT: OFF 33 | ROS: ON 34 | PERCEPTION: ON 35 | ``` 36 | 37 | addons in the docker: 38 | ``` 39 | #apt-get install qtcreator 40 | cd ~/local/LGP-execution 41 | 42 | apt-get install \ 43 | libglew-dev \ 44 | g++ liblapack-dev libf2c2-dev gnuplot libjsoncpp-dev libx11-dev \ 45 | libann-dev \ 46 | libassimp-dev libglew-dev libqhull-dev \ 47 | libglew-dev \ 48 | libx11-dev libpng12-dev freeglut3-dev libglew-dev graphviz-dev \ 49 | ros-kinetic-vrpn-client-ros emacs mesa-utils x11-apps iputils-ping less nmap net-tools 50 | ``` 51 | Alternatively, incorporate into `spartan.dockerfile`: 52 | (put above in `install_dependencies_lgp.sh` at spartan/setup/docker) 53 | ``` 54 | COPY ./setup/docker/install_dependencies_lgp.sh /tmp/install_dependencies_lgp.sh 55 | RUN yes "Y" | /tmp/install_dependencies_lgp.sh 56 | ``` 57 | 58 | 59 | saving and starting docker images 60 | ``` 61 | docker commit spartan spartan-after-compile-marc:2 62 | setup/docker/docker_run.py --container spartan-after-compile-marc 63 | ``` 64 | 65 | Getting communication with OptiTrack: 66 | ``` 67 | In the MotiveTracker: Broadcast ON; use 'Local Interface' URL!; VRPN Streaming ON 68 | vrpn_print_devices RigidBody01@128.30.27.150 69 | roslaunch vrpn_client_ros sample.launch server:=128.30.27.150 70 | ``` 71 | 72 | launching things in the docker: 73 | ``` 74 | make catkin-projects/fast 75 | 76 | use_ros 77 | use_spartan 78 | kuka_iiwa_procman & 79 | 80 | apps/iiwa/runprocman.sh 81 | 82 | ``` 83 | 84 | 85 | Help on `rai`: https://github.com/MarcToussaint/rai-maintenance/tree/master/help 86 | -------------------------------------------------------------------------------- /retired/help/bashrc: -------------------------------------------------------------------------------- 1 | export ROSDIR=/opt/ros/kinetic 2 | source ${ROSDIR}/setup.bash 3 | #export ROS_PACKAGE_PATH=${HOME}/local -------------------------------------------------------------------------------- /retired/help/docker_build.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from __future__ import print_function 3 | 4 | import argparse 5 | import os 6 | import getpass 7 | 8 | if __name__=="__main__": 9 | 10 | print("building docker container . . . ") 11 | user_name = getpass.getuser() 12 | default_image_name = user_name + "-spartan" 13 | 14 | 15 | parser = argparse.ArgumentParser() 16 | parser.add_argument("-i", "--image", type=str, 17 | help="name for the newly created docker image", default=default_image_name) 18 | 19 | parser.add_argument("-d", "--dry_run", action='store_true', help="(optional) perform a dry_run, print the command that would have been executed but don't execute it.") 20 | 21 | parser.add_argument("-p", "--password", type=str, 22 | help="(optional) password for the user", default="password") 23 | 24 | parser.add_argument('-uid','--user_id', type=int, help="(optional) user id for this user", default=os.getuid()) 25 | parser.add_argument('-gid','--group_id', type=int, help="(optional) user gid for this user", default=os.getgid()) 26 | 27 | args = parser.parse_args() 28 | print("building docker image named ", args.image) 29 | cmd = "docker build --build-arg USER_NAME=%(user_name)s \ 30 | --build-arg USER_PASSWORD=%(password)s \ 31 | --build-arg USER_ID=%(user_id)s \ 32 | --build-arg USER_GID=%(group_id)s" \ 33 | %{'user_name': user_name, 'password': args.password, 'user_id': args.user_id, 'group_id': args.group_id} 34 | cmd += " --network=host" 35 | cmd += " -t %s -f setup/docker/spartan.dockerfile ." % args.image 36 | 37 | 38 | print("command = \n \n", cmd) 39 | print("") 40 | 41 | # build the docker image 42 | if not args.dry_run: 43 | print("executing shell command") 44 | os.system(cmd) 45 | else: 46 | print("dry run, not executing command") -------------------------------------------------------------------------------- /retired/help/docker_build_lgp.sh: -------------------------------------------------------------------------------- 1 | 2 | #/bin/sh 3 | 4 | docker build \ 5 | --tag christina-lgp \ 6 | --network host \ 7 | -f full.dockerfile . 8 | -------------------------------------------------------------------------------- /retired/help/docker_run_lgp.sh: -------------------------------------------------------------------------------- 1 | #/bin/sh 2 | 3 | thispath=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd) 4 | 5 | xhost +local:root 6 | 7 | #nvidia-docker run --name spartan -e DISPLAY -e QT_X11_NO_MITSHM=1 -v /tmp/.X11-unix:/tmp/.X11-unix:rw -v /home/christina/spartan:/home/christina/spartan -v ~/.ssh:/home/christina/.ssh -v ~/.spartan-docker/christina-spartan-build:/home/christina/.spartan-build --user christina -v /home/christina/data/spartan:/home/christina/spartan/data_volume -p 11311:11311 --network host --privileged -v /dev/bus/usb:/dev/bus/usb --rm --ulimit rtprio=30 -it christina-spartan 8 | 9 | docker run -it \ 10 | --volume="$thispath/../..:/root/local" \ 11 | --volume="$thispath/docker.bashrc:/root/bashrc" \ 12 | --volume="$HOME/.gitconfig:/root/.gitconfig:ro" \ 13 | --volume="$HOME/.ssh:/root/.ssh:ro" \ 14 | --env="DISPLAY" \ 15 | --publish 11311:11311 \ 16 | --network host \ 17 | christina-lgp /bin/bash 18 | 19 | xhost -local:root 20 | 21 | 22 | -------------------------------------------------------------------------------- /retired/help/install_dependencies_lgp.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | apt-get install \ 4 | libglew-dev \ 5 | g++ liblapack-dev libf2c2-dev gnuplot libjsoncpp-dev libx11-dev \ 6 | libann-dev \ 7 | libassimp-dev libglew-dev libqhull-dev \ 8 | libglew-dev \ 9 | libx11-dev libpng12-dev freeglut3-dev libglew-dev graphviz-dev \ 10 | ros-kinetic-vrpn-client-ros emacs mesa-utils x11-apps iputils-ping less nmap net-tools 11 | -------------------------------------------------------------------------------- /retired/help/spartanlgp.dockerfile: -------------------------------------------------------------------------------- 1 | FROM nvidia/cuda:8.0-devel-ubuntu16.04 2 | 3 | RUN apt-get update 4 | RUN apt install sudo 5 | 6 | 7 | 8 | # working directory is /root 9 | WORKDIR /root 10 | 11 | COPY ./setup/docker/install_dependencies.sh /tmp/install_dependencies.sh 12 | RUN yes "Y" | /tmp/install_dependencies.sh 13 | 14 | COPY ./setup/ubuntu/16.04/install_prereqs.sh /tmp/spartan_install_prereqs.sh 15 | RUN yes "Y" | /tmp/spartan_install_prereqs.sh 16 | 17 | #COPY ./drake/setup/ubuntu/16.04/install_prereqs.sh /tmp/drake_install_prereqs.sh 18 | #RUN yes "Y" | /tmp/drake_install_prereqs.sh 19 | 20 | # Hack needed to deal with bazel issue, see https://github.com/bazelbuild/bazel/issues/4483 21 | COPY ./setup/docker/install_dependencies_drake.sh /tmp/drake_install_prereqs.sh 22 | RUN yes "Y" | /tmp/drake_install_prereqs.sh 23 | 24 | COPY ./director/distro/travis/install_deps.sh /tmp/director_travis_install_prereqs.sh 25 | RUN yes "Y" | TRAVIS_OS_NAME=linux /tmp/director_travis_install_prereqs.sh 26 | 27 | # install handical 28 | COPY ./setup/docker/install_handical_dependencies.sh /tmp/install_handical_dependencies.sh 29 | RUN yes "Y" | /tmp/install_handical_dependencies.sh 30 | 31 | #install Open3D 32 | COPY ./src/Open3D/scripts/install-deps-ubuntu.sh /tmp/open3d-install-deps-ubuntu.sh 33 | RUN yes "Y" | /tmp/open3d-install-deps-ubuntu.sh 34 | 35 | # set the terminator inside the docker container to be a different color 36 | RUN mkdir -p .config/terminator 37 | COPY ./setup/docker/terminator_config .config/terminator/config 38 | #RUN chown $USER_NAME:$USER_NAME -R .config 39 | 40 | 41 | # setup bazel bashrc 42 | # RUN echo "startup --output_base=/root/.spartan-build" >> .bazelrc 43 | RUN echo "startup --output_base=/root/.spartan-build" >> /etc/bazel.bazelrc 44 | 45 | COPY ./setup/docker/install_dependencies_lgp.sh /tmp/install_dependencies_lgp.sh 46 | RUN yes "Y" | /tmp/install_dependencies_lgp.sh 47 | 48 | ENTRYPOINT bash -c "source ~/spartan/setup/docker/entrypoint.sh && /bin/bash" 49 | 50 | 51 | -------------------------------------------------------------------------------- /retired/model/LGP-kuka.g: -------------------------------------------------------------------------------- 1 | Include='problem-01.g' 2 | 3 | #frame box0 { shape=ssBox size=[.05 .05 .16 .01] color=[.7 .1 .1] X= percept } 4 | 5 | shape(stick){ type=marker size =[.1] } 6 | shape(endeff){ type=marker size=[.1 ] } 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /retired/model/check.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 2, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import sys\n", 10 | "sys.path.append('../rai/rai/ry')\n", 11 | "from libry import *\n", 12 | "from numpy import *" 13 | ] 14 | }, 15 | { 16 | "cell_type": "code", 17 | "execution_count": 2, 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "C = Config()\n", 22 | "V = C.view()" 23 | ] 24 | }, 25 | { 26 | "cell_type": "code", 27 | "execution_count": 3, 28 | "metadata": {}, 29 | "outputs": [], 30 | "source": [ 31 | "C.addFile('pandaStation-noTilt.g')" 32 | ] 33 | }, 34 | { 35 | "cell_type": "code", 36 | "execution_count": 4, 37 | "metadata": {}, 38 | "outputs": [ 39 | { 40 | "name": "stdout", 41 | "output_type": "stream", 42 | "text": [ 43 | "[ 0. 0. -1. -1. 0. 0. -2. -2. 0. 0. 2. 2. 0.\n", 44 | " 0. 0.05 0.05]\n" 45 | ] 46 | } 47 | ], 48 | "source": [ 49 | "q = C.getJointState()\n", 50 | "print(q)" 51 | ] 52 | }, 53 | { 54 | "cell_type": "code", 55 | "execution_count": 12, 56 | "metadata": {}, 57 | "outputs": [], 58 | "source": [ 59 | "q = [.0, .0, 1., 1., -0.1, 0.1, -1.3, -1.3, -1.6, 1.6, 1.7, 1.7, -0., -1.6, 0.05, 0.05]\n", 60 | "C.setJointState(q)" 61 | ] 62 | }, 63 | { 64 | "cell_type": "code", 65 | "execution_count": 28, 66 | "metadata": {}, 67 | "outputs": [], 68 | "source": [ 69 | "V=0\n", 70 | "C=0" 71 | ] 72 | }, 73 | { 74 | "cell_type": "code", 75 | "execution_count": null, 76 | "metadata": {}, 77 | "outputs": [], 78 | "source": [ 79 | "0.0528487 1.08027 0.0718301 -1.37395 1.57773 1.70617 -1.64092" 80 | ] 81 | } 82 | ], 83 | "metadata": { 84 | "kernelspec": { 85 | "display_name": "Python 3", 86 | "language": "python", 87 | "name": "python3" 88 | }, 89 | "language_info": { 90 | "codemirror_mode": { 91 | "name": "ipython", 92 | "version": 3 93 | }, 94 | "file_extension": ".py", 95 | "mimetype": "text/x-python", 96 | "name": "python", 97 | "nbconvert_exporter": "python", 98 | "pygments_lexer": "ipython3", 99 | "version": "3.5.2" 100 | } 101 | }, 102 | "nbformat": 4, 103 | "nbformat_minor": 2 104 | } 105 | -------------------------------------------------------------------------------- /retired/model/hook.g: -------------------------------------------------------------------------------- 1 | frame hook { X: } 2 | frame stick (hook) { 3 | shape=ssBox size=[.704 .025 .04 .01] color=[.6 .3 0] contact, percept, logical={ object } 4 | Q= } 5 | shape stickTip (hook) { rel= type=ssBox size=[.20 .026 .04 0.01] color=[.6 .3 0], logical={ object, pusher }, contact } 6 | -------------------------------------------------------------------------------- /retired/model/model.g: -------------------------------------------------------------------------------- 1 | Include='problem-01.g' 2 | 3 | #frame box0 { shape=ssBox size=[.05 .05 .16 .01] color=[.7 .1 .1] X= percept } 4 | -------------------------------------------------------------------------------- /retired/model/pandaSingle.g: -------------------------------------------------------------------------------- 1 | frame world { X: } 2 | frame box{ X: } 3 | frame base (box){ Q: } #d(45 0 1 0)> } 4 | 5 | Prefix: "R_" 6 | Include: '../rai-robotModels/panda/panda.g' 7 | Prefix! 8 | 9 | Edit R_panda_link0 (base) { Q: } 10 | Edit R_panda_joint2 { q= -.5 } 11 | 12 | frame endeffR (R_panda_hand_joint) { Q: shape:marker size:[.1] color:[1 0 1] } 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /retired/model/pandaStation/cameraCalibration.g: -------------------------------------------------------------------------------- 1 | Include = 'pandaStation.g' 2 | 3 | frame calibrationVolumeR(base) { shape:ssBox size:[.35 .25 .3 .01] color:[.5 0 0 .2] Q:, noVisual } 4 | frame calibrationVolumeL(base) { shape:ssBox size:[.35 .25 .3 .01] color:[.5 0 0 .2] Q:, noVisual } 5 | 6 | -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/.gitignore: -------------------------------------------------------------------------------- 1 | *.dae 2 | -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/collision/finger.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/retired/model/pandaStation/meshes/collision/finger.stl -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/collision/hand.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/retired/model/pandaStation/meshes/collision/hand.stl -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/collision/link0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/retired/model/pandaStation/meshes/collision/link0.stl -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/collision/link1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/retired/model/pandaStation/meshes/collision/link1.stl -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/collision/link2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/retired/model/pandaStation/meshes/collision/link2.stl -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/collision/link3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/retired/model/pandaStation/meshes/collision/link3.stl -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/collision/link4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/retired/model/pandaStation/meshes/collision/link4.stl -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/collision/link5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/retired/model/pandaStation/meshes/collision/link5.stl -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/collision/link6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/retired/model/pandaStation/meshes/collision/link6.stl -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/collision/link7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/retired/model/pandaStation/meshes/collision/link7.stl -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/visual/HOWTO.sh: -------------------------------------------------------------------------------- 1 | meshlabserver -i finger.dae -o finger.ply -s script.mlx -om 2 | meshlabserver -i hand.dae -o hand.ply -s script.mlx -om 3 | meshlabserver -i link0.dae -o link0.ply -s script.mlx -om 4 | meshlabserver -i link1.dae -o link1.ply -s script.mlx -om 5 | meshlabserver -i link2.dae -o link2.ply -s script.mlx -om 6 | meshlabserver -i link3.dae -o link3.ply -s script.mlx -om 7 | meshlabserver -i link4.dae -o link4.ply -s script.mlx -om 8 | meshlabserver -i link5.dae -o link5.ply -s script.mlx -om 9 | meshlabserver -i link6.dae -o link6.ply -s script.mlx -om 10 | meshlabserver -i link7.dae -o link7.ply -s script.mlx -om 11 | -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/visual/finger.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/retired/model/pandaStation/meshes/visual/finger.ply -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/visual/hand.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/retired/model/pandaStation/meshes/visual/hand.ply -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/visual/link0.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/retired/model/pandaStation/meshes/visual/link0.ply -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/visual/link1.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/retired/model/pandaStation/meshes/visual/link1.ply -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/visual/link2.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/retired/model/pandaStation/meshes/visual/link2.ply -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/visual/link3.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/retired/model/pandaStation/meshes/visual/link3.ply -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/visual/link4.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/retired/model/pandaStation/meshes/visual/link4.ply -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/visual/link5.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/retired/model/pandaStation/meshes/visual/link5.ply -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/visual/link6.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/retired/model/pandaStation/meshes/visual/link6.ply -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/visual/link7.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/retired/model/pandaStation/meshes/visual/link7.ply -------------------------------------------------------------------------------- /retired/model/pandaStation/meshes/visual/script.mlx: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /retired/model/pandaStation/panda.g: -------------------------------------------------------------------------------- 1 | Include='panda_clean.g' 2 | 3 | Include 'panda_simplerCollisionModels.g' 4 | 5 | ## zero position 6 | 7 | Edit panda_joint1 { q= 0.0 } 8 | Edit panda_joint2 { q= -1. } 9 | Edit panda_joint3 { q= 0. } 10 | Edit panda_joint4 { q= -2.} 11 | Edit panda_joint5 { q= -0. } 12 | Edit panda_joint6 { q= 2. } 13 | Edit panda_joint7 { q= 0.0 } 14 | Edit panda_finger_joint1 { q=.04 } 15 | -------------------------------------------------------------------------------- /retired/model/pandaStation/panda_simplerCollisionModels.g: -------------------------------------------------------------------------------- 1 | Delete panda_link0_0 2 | Delete panda_link1_0 3 | Delete panda_link2_0 4 | Delete panda_link3_0 5 | Delete panda_link4_0 6 | Delete panda_link5_0 7 | Delete panda_link6_0 8 | Delete panda_link7_0 9 | Delete panda_hand_0 10 | Delete panda_leftfinger_0 11 | Delete panda_rightfinger_0 12 | 13 | frame (panda_link0) { shape:capsule color:[ 0.8 0.2 0.2 0.2 ] size:[.1 .1] Q:, noVisual, contact:-2 } 14 | 15 | frame (panda_joint1) { shape:capsule color:[ 0.8 0.2 0.2 0.2 ] size:[.2 .08] Q:, noVisual, contact:-2 } 16 | frame (panda_joint3) { shape:capsule color:[ 0.8 0.2 0.2 0.2 ] size:[.2 .08] Q:, noVisual, contact:-2 } 17 | frame (panda_joint5) { shape:capsule color:[ 0.8 0.2 0.2 0.2 ] size:[.22 .08] Q:, noVisual, contact:-2 } 18 | 19 | frame (panda_joint2) { shape:capsule color:[ 0.8 0.2 0.2 0.2 ] size:[.12 .08] Q:, noVisual, contact:-2 } 20 | frame (panda_joint4) { shape:capsule color:[ 0.8 0.2 0.2 0.2 ] size:[.12 .08] Q:, noVisual, contact:-2 } 21 | frame (panda_joint6) { shape:capsule color:[ 0.8 0.2 0.2 0.2 ] size:[.1 .07] Q:, noVisual, contact:-2 } 22 | frame (panda_joint7) { shape:capsule color:[ 0.8 0.2 0.2 0.2 ] size:[.1 .07] Q:, noVisual, contact:-2 } 23 | 24 | frame handCollision (panda_hand_joint) { shape:capsule color:[ 0.8 0.2 0.2 0.2 ] size:[.15 .06] Q:, noVisual, contact:-3 } 25 | 26 | #frame panda_leftfingerCollision(panda_finger_joint1) { shape:capsule color:[ 0.8 0.2 0.2 0.2 ] size:[.03 .014] Q:, noVisual, contact:-2 } 27 | #frame panda_rightfingerCollision(panda_finger_joint2) { shape:capsule color:[ 0.8 0.2 0.2 0.2 ] size:[.03 .014] Q:, noVisual, contact:-2 } 28 | 29 | 30 | frame panda_leftfingerCollision(panda_hand_joint) { shape:capsule color:[ 0.8 0.2 0.2 0.2 ] size:[.07 .017] Q:, noVisual, contact:-2 } 31 | frame panda_rightfingerCollision(panda_hand_joint) { shape:capsule color:[ 0.8 0.2 0.2 0.2 ] size:[.07 .017] Q:, noVisual, contact:-2 } 32 | -------------------------------------------------------------------------------- /retired/model/problem-01.g: -------------------------------------------------------------------------------- 1 | Include = 'problem-shared.g' 2 | 3 | ### ball 4 | 5 | frame Hook { X: } 6 | frame KukaBase { } 7 | 8 | joint (KukaBase base) { type:rigid, B: } 9 | 10 | ### hook 11 | 12 | #frame stick { type=5 size=[.2 .2 .2] } 13 | #joint (table1 nostick) { to= type=rigid} 14 | 15 | frame table1 (KukaBase){ type=ssBox, Q=, size=[1.914 1.83 .2 .02], color=[.3 .3 .3], contact, percept, logical={ table } } 16 | 17 | frame stick (Hook) { 18 | shape=ssBox size=[.704 .025 .04 .01] color=[.6 .3 0] contact, percept, logical={ object } 19 | Q= } 20 | shape stickTip (Hook) { rel= type=ssBox size=[.20 .026 .04 0.01] color=[.6 .3 0], logical={ object, pusher }, contact } 21 | 22 | frame redBall(table1) { 23 | shape=ssBox, size=[.06 .06 .06 .02] color=[1 0 0], 24 | contact, percept, logical={ object }, 25 | joint=rigid, Q= 26 | } 27 | 28 | frame RigidBody01(table1) { 29 | shape=ssBox, size=[.06 .06 .06 .02] color=[1 1 0], 30 | contact, percept, logical={ object }, 31 | joint=rigid, Q= 32 | } 33 | 34 | -------------------------------------------------------------------------------- /retired/model/problem-shared.g: -------------------------------------------------------------------------------- 1 | #Include = '../../data/baxter_model/baxter.ors' 2 | Include='../rai-robotModels/kuka_drake/setup.g' 3 | 4 | Edit base { X= } # for kuka 5 | #Edit base_footprint { X= } # for baxter 6 | 7 | #body slider1a { type=box size=[.2 .02 .02 0] color=[.5 .5 .5] } 8 | #body slider1b { type=box size=[.2 .02 .02 0] color=[.8 .3 .3] } 9 | #joint slider1Joint(slider1a slider1b){ type=transX } 10 | #shape (slider1b){ rel= type=5 size=[.1 .1 .1] color=[0 1 0] } 11 | 12 | 13 | Edit endeff { logical={ gripper, free } } 14 | 15 | -------------------------------------------------------------------------------- /retired/model/stick.g: -------------------------------------------------------------------------------- 1 | frame hook { X: } 2 | frame stick (hook) { 3 | shape=ssBox size=[.704 .025 .04 .01] color=[.6 .3 0] contact, percept, logical={ object } 4 | Q= } 5 | -------------------------------------------------------------------------------- /retired/operate/franka_cmdLine/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | 4 | DEPEND = Core Gui Geo Franka Kin Control 5 | 6 | include $(BASE)/_make/generic.mk 7 | -------------------------------------------------------------------------------- /retired/operate/kinematicsTest/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../../rai 2 | BASE2 = ../../../src 3 | 4 | DEPEND = Core Gui Geo Kin KOMO Operate Perception RealSense 5 | 6 | OPENCV = 1 7 | 8 | include $(BASE)/_make/generic.mk 9 | -------------------------------------------------------------------------------- /retired/operate/kinematicsTest/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | //=========================================================================== 12 | 13 | void test_pickAndPlace(){ 14 | rai::KinematicWorld K; 15 | K.addFile("../../model/pandaStation-noTilt.g"); 16 | 17 | //-- robot simulation 18 | SimulationThread R(K); 19 | 20 | //============== 21 | // 22 | // add the hook now 23 | // 24 | 25 | // R.addFile("../../model/hook.g", "table", rai::Transformation({.5,.7,.01}, 0)); 26 | R.addFile("../../model/hook.g", "table", rai::Transformation({.3,.3,.01}, 0)); 27 | 28 | 29 | //cheating! in real I could not copy the real world! 30 | R.K.waitForNextRevision(1); 31 | K = R.K.get(); 32 | 33 | 34 | 35 | //============== 36 | // 37 | // what follows is a little script to make the robot do things 38 | // 39 | 40 | const char* endeff="endeffL"; 41 | const char* object="stickTip"; 42 | 43 | arr x0 = K.getFrameState(); 44 | arr q0 = K.getJointState(); 45 | StringA joints = K.getJointNames(); 46 | K.watch(true); 47 | 48 | //compute a grasp 49 | chooseBoxGrasp(K, endeff, object); 50 | arr grasp = K.getJointState(); 51 | K.watch(true); 52 | 53 | //compute a path from x0 to grasp 54 | K.setFrameState(x0); 55 | auto path = computePath(K, grasp, joints, endeff, .0, .8); 56 | 57 | //open the gripper 58 | R.execGripper("pandaL", .1); 59 | R.waitForCompletion(); 60 | 61 | //execute the path 62 | R.executeMotion(joints, path.first, path.second); 63 | R.waitForCompletion(); 64 | 65 | //close gripper 66 | R.execGripper("pandaL", .0); 67 | R.waitForCompletion(); 68 | 69 | //attach 70 | R.attach(endeff, object); 71 | 72 | arr q_now = R.getJointPositions(joints); 73 | K.setJointState(q_now); 74 | K.watch(true); 75 | 76 | path = computePath(K, q0, joints, endeff, .2, .8); 77 | R.executeMotion(joints, path.first, path.second); 78 | R.waitForCompletion(); 79 | 80 | rai::wait(); 81 | } 82 | 83 | //=========================================================================== 84 | 85 | int main(int argc,char** argv){ 86 | rai::initCmdLine(argc,argv); 87 | 88 | test_pickAndPlace(); 89 | 90 | return 0; 91 | } 92 | 93 | 94 | -------------------------------------------------------------------------------- /retired/operate/operate/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | 4 | DEPEND = Core Gui Geo Kin KOMO Operate Perception RealSense FlatVision NewControl Franka LGPop 5 | 6 | OPENCV = 1 7 | 8 | include $(BASE)/_make/generic.mk 9 | -------------------------------------------------------------------------------- /retired/perception/calibObjects/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | 4 | DEPEND = Core Algo Gui Geo Kin RealSense Perception Control Franka LGPop opencv_reg FlatVision 5 | 6 | OPENCV = 1 7 | 8 | include $(BASE)/_make/generic.mk 9 | -------------------------------------------------------------------------------- /retired/perception/calibObjects/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | //=========================================================================== 7 | 8 | void online(){ 9 | LGPop OP(LGPop::SimulationMode); 10 | 11 | //-- that's the "working" config, 12 | rai::KinematicWorld K(OP.rawModel); 13 | StringA joints = K.getJointNames(); 14 | 15 | // OP.runRobotControllers(); 16 | OP.runTaskController(1); 17 | OP.runCamera(0); 18 | OP.runPerception(1); 19 | 20 | rai::wait(); 21 | } 22 | 23 | //=========================================================================== 24 | 25 | int main(int argc, char * argv[]){ 26 | 27 | online(); 28 | 29 | return 0; 30 | } 31 | -------------------------------------------------------------------------------- /retired/perception/calibObjects/rai.cfg: -------------------------------------------------------------------------------- 1 | RealSense/lowResolution:0 2 | RealSense/longCable:0 3 | RealSense/autoExposure:1 4 | -------------------------------------------------------------------------------- /retired/perception/calibration/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | 4 | DEPEND = Core Algo Gui Geo Kin RealSense Perception NewControl Franka LGPop FlatVision opencv_reg 5 | 6 | OPENCV = 1 7 | 8 | include $(BASE)/_make/generic.mk 9 | -------------------------------------------------------------------------------- /retired/perception/cameraCalibration/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | 4 | DEPEND = Core Algo Gui Geo KOMO Kin LGP Logic Perception RealSense NewControl Franka LGPop FlatVision opencv_reg RosCom 5 | 6 | OBJS = main.o cameraCalibration.o 7 | 8 | OPENCV = 1 9 | 10 | 11 | include $(BASE)/_make/generic.mk 12 | -------------------------------------------------------------------------------- /retired/perception/cameraCalibration/cameraCalibrationData_left_X.data: -------------------------------------------------------------------------------- 1 | -0.173894 0.169752 0.982519 2 | -0.175078 0.168983 1.08384 3 | -0.175277 0.249408 0.892058 4 | -0.175653 0.251421 0.988031 5 | -0.175707 0.252027 1.08679 6 | -0.176211 0.332885 0.890379 7 | -0.17508 0.334005 1.08283 8 | -0.176663 0.414814 0.889537 9 | -0.17554 0.417521 0.988633 10 | -0.175159 0.417015 1.08645 11 | -0.175853 0.418318 1.18669 12 | -0.0599478 0.166377 0.894572 13 | -0.056094 0.159833 0.987469 14 | -0.0554994 0.160103 1.08864 15 | -0.0576708 0.162035 1.18937 16 | -0.0602598 0.248833 0.890332 17 | -0.0558221 0.247474 0.989315 18 | -0.0550872 0.246849 1.09109 19 | -0.0596815 0.332977 0.892048 20 | -0.0557681 0.332379 0.991708 21 | -0.0557683 0.330265 1.08574 22 | -0.0599919 0.415031 0.992907 23 | -0.0569325 0.415198 1.08962 24 | -0.0574389 0.415134 1.19167 25 | 0.0572898 0.166865 0.895065 26 | 0.0620844 0.161192 1.09002 27 | 0.0599131 0.162734 1.19064 28 | 0.0546989 0.250937 0.887786 29 | 0.0622254 0.246086 1.08671 30 | 0.0614885 0.248084 1.19197 31 | 0.0540593 0.334084 0.887015 32 | 0.0601939 0.332833 0.99011 33 | 0.0610729 0.331683 1.18824 34 | 0.0529138 0.41933 0.886965 35 | 0.0595764 0.417402 0.990032 36 | 0.061197 0.415055 1.18918 37 | 0.173912 0.168023 0.894924 38 | 0.177891 0.159731 0.986621 39 | 0.179625 0.16106 1.19176 40 | 0.170039 0.250026 0.884341 41 | 0.177108 0.247803 0.987139 42 | 0.177668 0.247927 1.09063 43 | 0.178032 0.248142 1.19233 44 | 0.17042 0.334007 0.885975 45 | 0.176614 0.331386 0.987535 46 | 0.170551 0.418842 0.88811 47 | 0.176943 0.413431 0.986303 48 | 0.177427 0.413267 1.08451 -------------------------------------------------------------------------------- /retired/perception/cameraCalibration/cameraCalibrationData_left_x.data: -------------------------------------------------------------------------------- 1 | 104.893 207.567 0.839 2 | 81.3771 192.648 0.736 3 | 124.155 177.919 0.93 4 | 102.102 162.174 0.831 5 | 79.9571 147.297 0.733 6 | 123.808 132.767 0.93 7 | 80.9541 103.545 0.736 8 | 123.358 88.2661 0.928 9 | 101.513 72.3063 0.829 10 | 79.7492 57.9946 0.731 11 | 56.8304 42.5888 0.631 12 | 185.238 221.445 0.925 13 | 166.638 211.563 0.833 14 | 144.302 196.197 0.731 15 | 120.532 180.485 0.631 16 | 185.757 177.692 0.928 17 | 165.818 163.728 0.829 18 | 143.752 149.063 0.728 19 | 185.339 131.806 0.925 20 | 164.963 117.521 0.825 21 | 144.305 104.987 0.733 22 | 162.671 72.832 0.825 23 | 142.575 58.5921 0.728 24 | 119.128 43.4656 0.625 25 | 247.628 220.333 0.923 26 | 206.312 194.576 0.728 27 | 183.33 179.626 0.63 28 | 247.057 175.928 0.928 29 | 206.867 149.631 0.731 30 | 183.339 133.236 0.627 31 | 246.743 131.541 0.928 32 | 226.892 116.968 0.825 33 | 182.954 88.4415 0.628 34 | 246.459 85.6394 0.928 35 | 227.327 71.8334 0.827 36 | 183.924 43.8509 0.63 37 | 311.632 220.889 0.928 38 | 292.15 210.91 0.833 39 | 245.236 179.981 0.63 40 | 309.983 177.11 0.933 41 | 291.763 163.518 0.833 42 | 269.083 148.274 0.73 43 | 247.089 133.423 0.63 44 | 309.715 131.482 0.93 45 | 290.992 118.276 0.831 46 | 310.467 85.8275 0.93 47 | 291.859 74.2868 0.833 48 | 269.941 59.8394 0.734 -------------------------------------------------------------------------------- /retired/perception/cameraCalibration/cameraCalibrationData_right_X.data: -------------------------------------------------------------------------------- 1 | -0.173709 0.163247 0.888101 2 | -0.177249 0.159488 0.988916 3 | -0.177645 0.159309 1.08445 4 | -0.178645 0.160604 1.18992 5 | -0.171163 0.247809 0.884193 6 | -0.177712 0.246431 0.988439 7 | -0.176551 0.244967 1.07982 8 | -0.177853 0.246941 1.18988 9 | -0.170993 0.332041 0.883152 10 | -0.175602 0.329841 0.981103 11 | -0.178046 0.327804 1.08008 12 | -0.17134 0.417345 0.884818 13 | -0.176335 0.412816 0.981746 14 | -0.177781 0.411115 1.08021 15 | -0.176407 0.414448 1.18765 16 | -0.0580033 0.163068 0.886147 17 | -0.0592486 0.15701 0.985279 18 | -0.0608364 0.158746 1.08477 19 | -0.0548706 0.247841 0.884625 20 | -0.059973 0.246164 0.983867 21 | -0.0611213 0.244183 1.08264 22 | -0.0554874 0.330545 0.885153 23 | -0.0600807 0.330573 0.987733 24 | -0.0600427 0.330302 1.08444 25 | -0.0619015 0.330096 1.18355 26 | -0.0545835 0.417378 0.884664 27 | -0.0592535 0.417153 0.987438 28 | -0.0608969 0.413847 1.08783 29 | -0.0599404 0.415419 1.18372 30 | 0.0566843 0.16186 0.884154 31 | 0.0576477 0.159768 0.983582 32 | 0.0575705 0.159933 1.08265 33 | 0.0581411 0.160842 1.18676 34 | 0.0576335 0.243605 0.885298 35 | 0.0569294 0.247512 0.984668 36 | 0.0563736 0.246071 1.08607 37 | 0.0589502 0.330471 0.888636 38 | 0.0571993 0.333089 0.989027 39 | 0.0550934 0.331165 1.18926 40 | 0.0600629 0.414575 0.885632 41 | 0.0570343 0.415951 0.984677 42 | 0.0558771 0.414157 1.18944 43 | 0.173313 0.170836 0.888055 44 | 0.174227 0.244611 0.889504 45 | 0.174723 0.249574 0.9869 46 | 0.174722 0.249646 1.08532 47 | 0.174883 0.251298 1.18693 48 | 0.175576 0.327317 0.885097 49 | 0.174861 0.332178 0.987287 50 | 0.174044 0.331997 1.08417 51 | 0.173728 0.332605 1.1837 52 | 0.176514 0.412056 0.887167 53 | 0.176272 0.415639 0.986203 54 | 0.175367 0.414777 1.08349 -------------------------------------------------------------------------------- /retired/perception/cameraCalibration/cameraCalibrationData_right_x.data: -------------------------------------------------------------------------------- 1 | 124.302 225.696 0.93 2 | 99.6182 213.175 0.829 3 | 77.761 199.118 0.733 4 | 53.4218 183.05 0.627 5 | 126.118 180.501 0.933 6 | 99.3128 166.906 0.831 7 | 79.0449 154.022 0.738 8 | 53.3088 136.521 0.625 9 | 126.126 135.377 0.933 10 | 101.062 122.066 0.833 11 | 77.74 109.216 0.736 12 | 125.041 88.9289 0.928 13 | 100.602 77.4517 0.833 14 | 76.929 63.713 0.731 15 | 53.3966 46.5361 0.624 16 | 187.091 225.909 0.933 17 | 164.033 214.6 0.833 18 | 140.981 199.155 0.734 19 | 188.699 180.173 0.933 20 | 163.571 166.524 0.833 21 | 140.435 153.089 0.733 22 | 187.113 135.208 0.928 23 | 161.869 120.469 0.827 24 | 140.057 106.517 0.73 25 | 116.724 92.4186 0.631 26 | 187.595 88.7279 0.928 27 | 161.674 73.8505 0.825 28 | 138.829 61.2861 0.727 29 | 117.372 46.4653 0.63 30 | 247.813 225.289 0.93 31 | 226.94 212.424 0.833 32 | 204.615 198.033 0.734 33 | 181.78 182.75 0.631 34 | 247.834 181.296 0.928 35 | 225.617 165.233 0.831 36 | 202.871 151.254 0.73 37 | 246.359 133.612 0.92 38 | 223.771 118.224 0.823 39 | 177.851 90.4836 0.624 40 | 249.018 89.7704 0.928 41 | 224.938 74.619 0.829 42 | 177.662 46.059 0.623 43 | 310.853 220.707 0.93 44 | 311.327 180.8 0.93 45 | 289.268 163.734 0.831 46 | 267.384 149.694 0.733 47 | 244.018 133.174 0.631 48 | 311.593 136.538 0.93 49 | 288.715 119.055 0.829 50 | 266.126 104.932 0.731 51 | 243.644 90.4462 0.632 52 | 311.537 90.7156 0.928 53 | 289.291 74.2056 0.829 54 | 267.393 60.839 0.733 -------------------------------------------------------------------------------- /retired/perception/cameraCalibration/cameraCalibrationData_simulation_left_X.data: -------------------------------------------------------------------------------- 1 | [-0.174912, 0.166118, 0.890103, 2 | -0.175001, 0.165027, 0.989755, 3 | -0.175003, 0.165018, 1.08975, 4 | -0.175003, 0.165015, 1.18973, 5 | -0.175031, 0.248546, 0.89103, 6 | -0.175001, 0.24835, 0.989743, 7 | -0.17501, 0.248422, 1.18947, 8 | -0.175023, 0.331858, 0.890991, 9 | -0.175001, 0.331675, 0.989734, 10 | -0.175001, 0.331677, 1.08972, 11 | -0.175011, 0.41509, 0.890601, 12 | -0.175001, 0.414995, 0.989737, 13 | -0.175001, 0.415002, 1.08972, 14 | -0.175002, 0.415011, 1.18971, 15 | -0.0585054, 0.166026, 0.890597, 16 | -0.0583296, 0.165024, 0.989758, 17 | -0.0583374, 0.165087, 1.1895, 18 | -0.0582393, 0.248539, 0.891047, 19 | -0.058332, 0.248348, 0.989745, 20 | -0.0583352, 0.248348, 1.08974, 21 | -0.0583008, 0.331513, 0.990362, 22 | -0.0583357, 0.331739, 1.18945, 23 | -0.0582589, 0.415268, 0.890944, 24 | -0.0583367, 0.414991, 0.989733, 25 | -0.0583361, 0.415002, 1.08972, 26 | -0.0583351, 0.415011, 1.18971, 27 | 0.0582038, 0.16598, 0.890661, 28 | 0.0583383, 0.16502, 0.989754, 29 | 0.0583317, 0.165017, 1.08976, 30 | 0.0583269, 0.165016, 1.18973, 31 | 0.0585213, 0.248501, 0.891059, 32 | 0.0583498, 0.248407, 1.08951, 33 | 0.0583298, 0.24835, 1.18973, 34 | 0.0584969, 0.331851, 0.891007, 35 | 0.0583305, 0.331672, 0.989744, 36 | 0.0583372, 0.331735, 1.18946, 37 | 0.0585204, 0.415329, 0.890911, 38 | 0.0583338, 0.415032, 1.08947, 39 | 0.17483, 0.165849, 0.890493, 40 | 0.175002, 0.165016, 0.989747, 41 | 0.175013, 0.165081, 1.18947, 42 | 0.175244, 0.248468, 0.891026, 43 | 0.174998, 0.248339, 0.989738, 44 | 0.174998, 0.248344, 1.08973, 45 | 0.174999, 0.248351, 1.18972, 46 | 0.175255, 0.331879, 0.890961, 47 | 0.174989, 0.331659, 0.989738, 48 | 0.174994, 0.331671, 1.08972, 49 | 0.175001, 0.33168, 1.18971, 50 | 0.175541, 0.415829, 0.890671, 51 | 0.174946, 0.414922, 0.989756, 52 | 0.174982, 0.414981, 1.08972, 53 | 0.174999, 0.415006, 1.18971] -------------------------------------------------------------------------------- /retired/perception/cameraCalibration/cameraCalibrationData_simulation_left_x.data: -------------------------------------------------------------------------------- 1 | [155.771, 144.678, 0.924457, 2 | 133.206, 135.268, 0.824805, 3 | 110.994, 125.49, 0.724806, 4 | 88.4129, 115.44, 0.624826, 5 | 155.512, 118.34, 0.92353, 6 | 133.531, 108.571, 0.824817, 7 | 88.4499, 88.61, 0.625087, 8 | 155.404, 91.4333, 0.923569, 9 | 133.209, 81.6577, 0.824826, 10 | 111.009, 71.6503, 0.724838, 11 | 155.687, 64.6771, 0.923959, 12 | 133.449, 54.8507, 0.824822, 13 | 110.901, 44.7332, 0.724843, 14 | 88.4163, 34.9916, 0.62485, 15 | 193.108, 145.062, 0.923963, 16 | 170.83, 135.363, 0.824802, 17 | 126.032, 115.406, 0.625058, 18 | 193.014, 118.21, 0.923512, 19 | 170.826, 108.571, 0.824815, 20 | 148.382, 98.576, 0.724823, 21 | 170.81, 81.5956, 0.824198, 22 | 126.085, 61.8263, 0.625106, 23 | 193.036, 64.6531, 0.923616, 24 | 170.739, 54.851, 0.824827, 25 | 148.405, 44.8933, 0.724839, 26 | 126.034, 34.9326, 0.624852, 27 | 230.975, 145.052, 0.923899, 28 | 208.58, 135.364, 0.824805, 29 | 185.912, 125.391, 0.724803, 30 | 163.647, 115.409, 0.624834, 31 | 230.875, 118.208, 0.923502, 32 | 185.977, 98.6075, 0.725055, 33 | 163.527, 88.5476, 0.624832, 34 | 230.888, 91.4318, 0.923553, 35 | 208.477, 81.6567, 0.824816, 36 | 163.597, 61.706, 0.625097, 37 | 230.912, 64.6554, 0.923649, 38 | 185.986, 44.9558, 0.725093, 39 | 268.339, 144.95, 0.924067, 40 | 246.207, 135.269, 0.824813, 41 | 201.261, 115.365, 0.625085, 42 | 268.389, 118.341, 0.923534, 43 | 246.153, 108.488, 0.824822, 44 | 223.611, 98.5775, 0.724835, 45 | 201.198, 88.5968, 0.624838, 46 | 268.306, 91.4363, 0.923599, 47 | 246.209, 81.6574, 0.824822, 48 | 223.612, 71.7587, 0.724836, 49 | 201.202, 61.7299, 0.62485, 50 | 268.46, 64.2803, 0.923889, 51 | 245.963, 54.8495, 0.824804, 52 | 223.612, 44.9399, 0.724837, 53 | 201.202, 34.9917, 0.624852] -------------------------------------------------------------------------------- /retired/perception/cameraCalibration/cameraCalibrationData_simulation_right_X.data: -------------------------------------------------------------------------------- 1 | [-0.174533, 0.166307, 0.890386, 2 | -0.175001, 0.165025, 0.989769, 3 | -0.174996, 0.165022, 1.08976, 4 | -0.174991, 0.165022, 1.18975, 5 | -0.175244, 0.248479, 0.891038, 6 | -0.174996, 0.248338, 0.989739, 7 | -0.174997, 0.248345, 1.08973, 8 | -0.174997, 0.248352, 1.18973, 9 | -0.175253, 0.331882, 0.890964, 10 | -0.174989, 0.33166, 0.989741, 11 | -0.174995, 0.33167, 1.08972, 12 | -0.175001, 0.33168, 1.18971, 13 | -0.175538, 0.415826, 0.890674, 14 | -0.174946, 0.414922, 0.989756, 15 | -0.174983, 0.41498, 1.08972, 16 | -0.174999, 0.415006, 1.18971, 17 | -0.0587296, 0.165917, 0.890493, 18 | -0.0583592, 0.165086, 1.08954, 19 | -0.0583274, 0.165017, 1.18972, 20 | -0.0585201, 0.2485, 0.891051, 21 | -0.0583348, 0.248345, 0.989742, 22 | -0.0583316, 0.248346, 1.08973, 23 | -0.05833, 0.24835, 1.18972, 24 | -0.0584961, 0.331853, 0.891007, 25 | -0.0583307, 0.331669, 0.989735, 26 | -0.0583307, 0.331675, 1.08973, 27 | -0.0583321, 0.331681, 1.18971, 28 | -0.0585197, 0.415324, 0.890916, 29 | -0.0583227, 0.41498, 0.989737, 30 | -0.0583275, 0.414998, 1.08972, 31 | -0.0583322, 0.41501, 1.18971, 32 | 0.0580335, 0.165961, 0.890459, 33 | 0.0583282, 0.165026, 0.989763, 34 | 0.058335, 0.165017, 1.08976, 35 | 0.0583389, 0.165014, 1.18973, 36 | 0.0582389, 0.248542, 0.891064, 37 | 0.058332, 0.248348, 0.989742, 38 | 0.058335, 0.248348, 1.08973, 39 | 0.0583367, 0.248349, 1.18972, 40 | 0.0582567, 0.331861, 0.89101, 41 | 0.058334, 0.331673, 0.989734, 42 | 0.0583353, 0.331677, 1.08973, 43 | 0.0583356, 0.331682, 1.18971, 44 | 0.0582584, 0.415264, 0.890949, 45 | 0.0583368, 0.414992, 0.989735, 46 | 0.058336, 0.415001, 1.08972, 47 | 0.0583353, 0.415011, 1.18971, 48 | 0.1748, 0.16598, 0.890444, 49 | 0.175001, 0.165025, 0.989757, 50 | 0.175003, 0.165018, 1.08975, 51 | 0.175003, 0.165015, 1.18973, 52 | 0.175031, 0.248546, 0.891035, 53 | 0.175001, 0.248349, 0.98974, 54 | 0.175002, 0.248349, 1.08973, 55 | 0.175003, 0.24835, 1.18972, 56 | 0.175024, 0.331857, 0.890992, 57 | 0.175006, 0.33174, 1.08948, 58 | 0.17501, 0.415097, 0.890588, 59 | 0.175, 0.414993, 0.98973, 60 | 0.175001, 0.415002, 1.08972, 61 | 0.175002, 0.415011, 1.18971] -------------------------------------------------------------------------------- /retired/perception/cameraCalibration/cameraCalibrationData_simulation_right_x.data: -------------------------------------------------------------------------------- 1 | [155.723, 144.633, 0.924174, 2 | 133.204, 135.266, 0.824791, 3 | 110.993, 125.488, 0.724795, 4 | 88.4101, 115.437, 0.624806, 5 | 155.396, 118.211, 0.923522, 6 | 133.531, 108.572, 0.824821, 7 | 111.008, 98.4687, 0.724834, 8 | 88.4255, 88.6106, 0.624833, 9 | 155.164, 91.436, 0.923596, 10 | 133.208, 81.6571, 0.824819, 11 | 111.009, 71.6505, 0.72484, 12 | 88.4163, 61.8601, 0.62485, 13 | 155.213, 64.6721, 0.923887, 14 | 133.446, 54.8495, 0.824804, 15 | 110.9, 44.7328, 0.724837, 16 | 88.4164, 34.9916, 0.624851, 17 | 193.13, 145.079, 0.924067, 18 | 148.422, 125.428, 0.725019, 19 | 126, 115.402, 0.624835, 20 | 193.013, 118.209, 0.923509, 21 | 170.827, 108.571, 0.824818, 22 | 148.383, 98.5766, 0.724828, 23 | 126.002, 88.5353, 0.624842, 24 | 193.023, 91.4317, 0.923553, 25 | 170.94, 81.6577, 0.824826, 26 | 148.384, 71.7587, 0.724835, 27 | 126.034, 61.801, 0.62485, 28 | 193.042, 64.6551, 0.923644, 29 | 170.738, 54.8507, 0.824823, 30 | 148.405, 44.8931, 0.724837, 31 | 126.034, 34.9325, 0.62485, 32 | 230.666, 144.956, 0.924101, 33 | 208.578, 135.363, 0.824797, 34 | 185.912, 125.391, 0.724802, 35 | 163.646, 115.408, 0.624829, 36 | 230.874, 118.207, 0.923496, 37 | 208.679, 108.464, 0.824818, 38 | 185.918, 98.5764, 0.724827, 39 | 163.53, 88.5488, 0.62484, 40 | 230.887, 91.4314, 0.92355, 41 | 208.48, 81.6577, 0.824826, 42 | 185.92, 71.7587, 0.724835, 43 | 163.586, 61.7353, 0.624852, 44 | 230.903, 64.6528, 0.923611, 45 | 208.681, 54.8508, 0.824824, 46 | 185.922, 44.9402, 0.724842, 47 | 163.581, 34.9917, 0.624852, 48 | 268.353, 144.958, 0.924117, 49 | 246.204, 135.268, 0.824803, 50 | 223.603, 125.392, 0.724808, 51 | 201.135, 115.407, 0.624828, 52 | 268.284, 118.211, 0.923526, 53 | 246.152, 108.488, 0.82482, 54 | 223.609, 98.5763, 0.724826, 55 | 201.198, 88.597, 0.624839, 56 | 268.296, 91.4332, 0.923567, 57 | 223.687, 71.7829, 0.72508, 58 | 268.414, 64.6781, 0.923972, 59 | 246.212, 54.8512, 0.82483, 60 | 223.614, 44.9402, 0.724843, 61 | 201.201, 34.9915, 0.624848] -------------------------------------------------------------------------------- /retired/perception/cameraCalibration/cameraCalibration_X.data: -------------------------------------------------------------------------------- 1 | -0.173564 0.195794 0.891948 2 | -0.176283 0.187993 0.982564 3 | -0.178428 0.190025 1.18735 4 | -0.17061 0.27798 0.882343 5 | -0.176319 0.277511 0.986243 6 | -0.177807 0.274355 1.08235 7 | -0.170363 0.362271 0.884265 8 | -0.175966 0.360824 0.986452 9 | -0.177878 0.360176 1.08968 10 | -0.171178 0.446685 0.98428 11 | -0.176553 0.443808 1.08141 12 | -0.176319 0.444297 1.18323 13 | -0.0572301 0.190676 0.881745 14 | -0.060237 0.18837 0.985265 15 | -0.0603801 0.188927 1.08389 16 | -0.0613748 0.19101 1.18534 17 | -0.0537661 0.275122 0.880813 18 | -0.0595045 0.275364 0.980005 19 | -0.0607792 0.275945 1.08304 20 | -0.0606013 0.277427 1.18809 21 | -0.0546075 0.360923 0.88317 22 | -0.0582883 0.363771 0.987616 23 | -0.0609764 0.361659 1.09073 24 | -0.0551561 0.444576 0.883886 25 | -0.0585669 0.446372 0.987634 26 | 0.0567999 0.192795 0.881787 27 | 0.0578553 0.190814 0.982249 28 | 0.0570739 0.190421 1.08453 29 | 0.0570111 0.191708 1.18863 30 | 0.0587407 0.27772 0.887278 31 | 0.0576026 0.277613 0.981652 32 | 0.0559273 0.278319 1.08934 33 | 0.0556702 0.277869 1.1898 34 | 0.0610201 0.36277 0.887793 35 | 0.0572117 0.363255 0.987488 36 | 0.0571089 0.361632 1.08589 37 | 0.0606379 0.443455 0.885719 38 | 0.0583135 0.444619 0.981815 39 | 0.172496 0.195212 0.879255 40 | 0.175213 0.19872 0.984318 41 | 0.176257 0.199767 1.08351 42 | 0.174405 0.275816 0.886512 43 | 0.175219 0.278814 0.981494 44 | 0.175695 0.28188 1.08568 45 | 0.176407 0.282529 1.18319 46 | 0.174252 0.357942 0.883006 47 | 0.174396 0.364137 1.08497 48 | 0.177295 0.441396 0.88411 49 | 0.177132 0.446616 0.98367 50 | 0.174615 0.447615 1.18307 -------------------------------------------------------------------------------- /retired/perception/cameraCalibration/cameraCalibration_x.data: -------------------------------------------------------------------------------- 1 | 123.297 208.52 0.928 2 | 101.394 198.897 0.835 3 | 53.6815 168.262 0.63 4 | 126.573 164.717 0.933 5 | 100.11 150.318 0.831 6 | 77.5235 137.682 0.733 7 | 125.833 119.252 0.93 8 | 99.8475 105.163 0.829 9 | 75.2018 90.4607 0.725 10 | 102.183 59.2303 0.829 11 | 77.3602 46.4065 0.731 12 | 54.5951 31.3789 0.629 13 | 187.631 211.699 0.935 14 | 163.172 197.998 0.833 15 | 141.131 183.331 0.734 16 | 117.401 167.344 0.632 17 | 188.997 166.074 0.933 18 | 164.2 151.61 0.835 19 | 140.378 136.499 0.733 20 | 116.642 120.414 0.628 21 | 187.933 119.407 0.93 22 | 162.745 103.069 0.827 23 | 137.737 88.9328 0.722 24 | 187.147 74.5493 0.928 25 | 162.192 58.5917 0.825 26 | 248.95 209.836 0.935 27 | 227.357 196.585 0.835 28 | 203.825 182.043 0.733 29 | 180.175 165.928 0.628 30 | 247.559 162.873 0.925 31 | 226.193 149.807 0.833 32 | 200.616 133.268 0.723 33 | 178.113 119.129 0.624 34 | 248.059 117.161 0.923 35 | 224.01 102.761 0.825 36 | 202.012 89.4987 0.727 37 | 249.078 74.6911 0.928 38 | 225.488 59.8786 0.829 39 | 311.878 208.751 0.938 40 | 290.034 191.571 0.833 41 | 267.186 175.971 0.734 42 | 310.913 164.33 0.93 43 | 290.07 148.881 0.835 44 | 267.811 132.454 0.733 45 | 245.169 117.419 0.636 46 | 310.509 120.331 0.93 47 | 266.141 88.111 0.731 48 | 312.269 75.8012 0.93 49 | 289.407 58.1595 0.829 50 | 244.664 29.0139 0.634 -------------------------------------------------------------------------------- /retired/perception/cameraCalibration/rai.cfg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/retired/perception/cameraCalibration/rai.cfg -------------------------------------------------------------------------------- /retired/perception/cameraCalibration/world.g: -------------------------------------------------------------------------------- 1 | #Include = '../../model/pandaStation.g' 2 | 3 | Include = '../../model/pandaStation/pandaStation.g' 4 | 5 | #frame calibrationMarkerL(L_panda_hand_1) {shape:sphere Q: size:[0.015] visual color:[1 0 0]} 6 | #frame calibrationMarkerR(R_panda_hand_1) {shape:sphere Q: size:[0.015] visual color:[1 0 0]} 7 | 8 | frame calibrationVolumeR(base) { shape:ssBox size:[.35 .25 .3 .01] color:[.5 0 0 .2] Q:, noVisual } 9 | frame calibrationVolumeL(base) { shape:ssBox size:[.35 .25 .3 .01] color:[.5 0 0 .2] Q:, noVisual } 10 | 11 | #frame cameraField(base) { shape:ssBox size:[.8 .4 .6 .01] color:[0 0.5 0 .2] Q:, noVisual } 12 | -------------------------------------------------------------------------------- /retired/perception/fullPipeline/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | 4 | DEPEND = Core Gui Geo Kin KOMO Operate Perception RealSense BackgroundSubtraction Control Franka LGPop 5 | 6 | OPENCV = 1 7 | 8 | include $(BASE)/_make/generic.mk 9 | -------------------------------------------------------------------------------- /retired/perception/fullPipeline/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | #include 20 | 21 | #include 22 | 23 | //=========================================================================== 24 | 25 | void perceptionPipeline(){ 26 | LGPop OP(false); 27 | 28 | OP.runCamera(0); 29 | OP.runPerception(0); 30 | rai::wait(); 31 | 32 | 33 | // //cheating! in real I could not copy the real world! 34 | // K.set()->addFile("../../model/stick.g", "table", rai::Transformation({.0,-.5,.05}, 0)); 35 | 36 | // rai::wait(); 37 | 38 | } 39 | 40 | //=========================================================================== 41 | 42 | int main(int argc,char** argv){ 43 | rai::initCmdLine(argc,argv); 44 | 45 | perceptionPipeline(); 46 | 47 | return 0; 48 | } 49 | 50 | 51 | -------------------------------------------------------------------------------- /retired/perception/objectDetection/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | 4 | DEPEND = Core Gui Operate Perception RealSense BackgroundSubtraction 5 | 6 | OPENCV = 1 7 | 8 | include $(BASE)/_make/generic.mk 9 | -------------------------------------------------------------------------------- /retired/perception/objectDetection/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | void testBGSThread() { 12 | RealSenseThread RS; 13 | // BackgroundSubtractionThread bgs(RS.color); 14 | LOG(0) <<"recording background statistics.."; 15 | // bgs.learnBackgroundModel(100); 16 | LOG(0) <<"..done"; 17 | 18 | RS.depth.waitForNextRevision(); 19 | Depth2PointCloud cvt2pcl(RS.depth, RS.depth_fxycxy(0), RS.depth_fxycxy(1), RS.depth_fxycxy(2), RS.depth_fxycxy(3)); 20 | PointCloudViewer pcview(cvt2pcl.points, RS.color, 1.); 21 | 22 | Var cameraDepth(RS.depth); 23 | Var cameraColor(RS.color); 24 | CV_BackgroundSubstraction BS; 25 | BS.threshold = .01; 26 | BS.verbose = 1; 27 | auto depthpipeline = loop([&cameraDepth, &cameraColor, &BS](){ 28 | cameraDepth.waitForNextRevision(); 29 | floatA depth = cameraDepth.get(); 30 | byteA color = cameraColor.get(); 31 | byteA mask; 32 | mask.resize(depth.d0, depth.d1).setZero(); 33 | auto lock = cvMutex(); 34 | BS.compute(color, depth, mask); 35 | return 0; 36 | }); 37 | 38 | rai::wait(); 39 | } 40 | 41 | int main(int argc,char **argv){ 42 | rai::initCmdLine(argc, argv); 43 | testBGSThread(); 44 | return 0; 45 | } 46 | 47 | -------------------------------------------------------------------------------- /retired/perception/opencvTests/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | 4 | DEPEND = Core Gui Perception RealSense 5 | 6 | OPENCV = 1 7 | 8 | include $(BASE)/_make/generic.mk 9 | -------------------------------------------------------------------------------- /retired/perception/realsense/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | 3 | DEPEND = Core Gui 4 | 5 | UBUNTU_DEPEND = libglfw3-dev 6 | 7 | GL=1 8 | 9 | CXXFLAGS += -DRAI_REALSENSE `pkg-config --cflags realsense2` 10 | LIBS += `pkg-config --libs realsense2` 11 | 12 | include $(BASE)/_make/generic.mk 13 | -------------------------------------------------------------------------------- /retired/perception/realsense/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | int main(int argc, char * argv[]){ 7 | rs2::log_to_console(RS2_LOG_SEVERITY_ERROR); 8 | rs2::pipeline pipe; 9 | pipe.start(); 10 | 11 | OpenGL glDepth, glColor; 12 | 13 | arr _depth; 14 | byteA _color; 15 | int key; 16 | 17 | for(;;){ 18 | rs2::frameset data = pipe.wait_for_frames(); // Wait for next set of frames from the camera 19 | 20 | rs2::depth_frame depth = data.get_depth_frame(); // Find and colorize the depth data 21 | rs2::video_frame color = data.get_color_frame(); // Find the color data 22 | 23 | //#if 0 24 | _depth.resize(depth.get_height(), depth.get_width()); 25 | for(uint y=0;y<_depth.d0;y++) for(uint x=0;x<_depth.d1;x++){ 26 | double d = depth.get_distance(x,y); 27 | if(d>2.) d=2.; 28 | _depth(y,x) = d; 29 | } 30 | 31 | glDepth.resize(depth.get_width(), depth.get_height()); 32 | key = glDepth.displayGrey(_depth, false, 1.); 33 | if(key=='q') break; 34 | //#else 35 | _color.resize(color.get_height(), color.get_width(), 3); 36 | CHECK(color.get_bytes_per_pixel()==3,""); 37 | memmove(_color.p, color.get_data(), _color.N); 38 | 39 | glColor.resize(color.get_width(), color.get_height()); 40 | key = glColor.watchImage(_color, false, 1.); 41 | if(key=='q') break; 42 | //#endif 43 | } 44 | 45 | return EXIT_SUCCESS; 46 | } 47 | -------------------------------------------------------------------------------- /retired/perception/registration/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | 4 | DEPEND = Core Algo Gui Geo Kin RealSense Perception Control Franka LGPop opencv_reg FlatVision 5 | 6 | OPENCV = 1 7 | 8 | include $(BASE)/_make/generic.mk 9 | -------------------------------------------------------------------------------- /retired/perception/registration/rai.cfg: -------------------------------------------------------------------------------- 1 | RealSense/lowResolution:0 2 | RealSense/longCable:0 3 | RealSense/autoExposure:1 4 | -------------------------------------------------------------------------------- /retired/perception/simulateCam/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | 4 | DEPEND = Core Gui Geo Kin KOMO Operate Perception RealSense BackgroundSubtraction Franka Control 5 | 6 | OPENCV = 1 7 | 8 | include $(BASE)/_make/generic.mk 9 | -------------------------------------------------------------------------------- /src/Audio/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | NAME = $(shell basename `pwd`) 4 | OUTPUT = lib$(NAME).so 5 | 6 | PORTAUDIO = 1 7 | DEPEND = Core 8 | 9 | SRCS = $(shell find . -maxdepth 1 -name '*.cpp' ) 10 | OBJS = $(SRCS:%.cpp=%.o) 11 | 12 | include $(BASE)/_make/generic.mk 13 | -------------------------------------------------------------------------------- /src/Audio/audio.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace rai { 7 | //=========================================================================== 8 | 9 | struct SineSound{ 10 | float sampleRate; 11 | floatA notes; //four entries per note: (sin-buffer-step-size, amplitude, time, decay) 12 | floatA SIN; 13 | Mutex mutex; 14 | 15 | SineSound(float _sampleRate=16000); 16 | 17 | void addNote(int noteRelToC, float a=.1, float decay=0.0007); 18 | void addFreq(float freq, float a=.1, float decay=0.0007); 19 | void changeFreq(uint i, float freq); 20 | void changeAmp(uint i, float amp); 21 | void reset(); 22 | void clean(); 23 | float get(); 24 | }; 25 | 26 | //=========================================================================== 27 | 28 | struct Audio{ 29 | void *stream; 30 | Audio(SineSound& S); 31 | ~Audio(); 32 | }; 33 | 34 | //=========================================================================== 35 | 36 | struct Sound : SineSound{ 37 | Audio A; 38 | Sound() : A(*this){} 39 | }; 40 | 41 | extern Singleton sound; 42 | 43 | } //namespace 44 | -------------------------------------------------------------------------------- /src/BotOp/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = .. 3 | NAME = $(shell basename `pwd`) 4 | EXT = $(shell python3-config --extension-suffix) 5 | OUTPUT = lib$(NAME).so #$(EXT) 6 | 7 | DEPEND = Core Algo Optim Gui Geo Kin KOMO LGP Franka Control Logic ry Audio 8 | 9 | PYBIND = 1 10 | 11 | SRCS = $(shell find . -maxdepth 1 -name '*.cpp' ) 12 | OBJS = $(SRCS:%.cpp=%.o) 13 | 14 | include $(BASE)/_make/generic.mk 15 | -------------------------------------------------------------------------------- /src/BotOp/SecMPC_Stepper.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "bot.h" 4 | #include 5 | 6 | //=========================================================================== 7 | 8 | struct SecMPC_Stepper{ 9 | Metronome tic; 10 | uint stepCount = 0; 11 | ofstream fil; 12 | FrameL logPoses; 13 | 14 | SecMPC_Stepper( double cycleTime=.1) 15 | : tic(cycleTime){ 16 | fil.open(STRING("z.SecMPC.log")); 17 | // fil.open(STRING("z." < 4 | #include 5 | 6 | //=========================================================================== 7 | 8 | arr getLoopPath(rai::Configuration& C); 9 | void addBoxPickObjectives_botop(KOMO& komo, double time, rai::ArgWord dir, const char* boxName, const arr& boxSize, const char* gripperName, const char* palmName, const char* tableName); 10 | void addBoxPlaceObjectives_botop(KOMO& komo, double time, rai::ArgWord dir, const char* boxName, const arr& boxSize, const char* gripperName, const char* palmName); 11 | arr getBoxPnpKeyframes(const rai::Configuration& C, 12 | str pickDirection, str placeDirection, 13 | const char* boxName, const char* gripperName, const char* palmName, const char* tableName, 14 | const arr& qHome); 15 | arr getBoxPnpKeyframes_new(rai::Configuration& C, str graspDirection, str placeDirection, str box, str gripper, str palm, str table, const arr& qHome); 16 | 17 | //=========================================================================== 18 | 19 | void getGraspLinePlane(arr& xLine, arr& yzPlane, FeatureSymbol& xyScalarProduct, FeatureSymbol& xzScalarProduct, 20 | const rai::ArgWord& dir); 21 | -------------------------------------------------------------------------------- /src/BotOp/py-BotOp.h: -------------------------------------------------------------------------------- 1 | /* ------------------------------------------------------------------ 2 | Copyright (c) 2011-2020 Marc Toussaint 3 | email: toussaint@tu-berlin.de 4 | 5 | This code is distributed under the MIT License. 6 | Please see /LICENSE for details. 7 | -------------------------------------------------------------- */ 8 | 9 | #pragma once 10 | 11 | #ifdef RAI_PYBIND 12 | 13 | #include 14 | 15 | void init_BotOp(pybind11::module& m); 16 | 17 | #endif 18 | -------------------------------------------------------------------------------- /src/BotOp/tools.cpp: -------------------------------------------------------------------------------- 1 | #include "tools.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | Move_IK::Move_IK(BotOp& _bot, rai::Configuration& _C, int _askForOK) : bot(_bot), C(_C), askForOK(_askForOK){ 10 | komo.setConfig(C, true); 11 | komo.setTiming(1., 1, 1., 0); 12 | komo.addControlObjective({}, 0, 1e-1); 13 | } 14 | 15 | bool Move_IK::go(){ 16 | NLP_Solver sol; 17 | sol.setProblem(komo.nlp()); 18 | sol.opt.set_stopTolerance(1e-4); 19 | auto ret = sol.solve(); 20 | cout <<*ret <feasible){ 22 | return false; 23 | }else{ 24 | qT = komo.getPath_qOrg()[-1]; 25 | C.setJointState(qT); 26 | if(askForOK && C.view(true, "Move_IK\ngo?")=='q') return false; 27 | 28 | bot.moveTo(qT, 2.); 29 | for(;bot.getTimeToEnd()>0.;) bot.sync(C); 30 | } 31 | return true; 32 | } 33 | 34 | bool sense_HsvBlob(BotOp& bot, rai::Configuration& C, const char* camName, const char* blobName, const arr& hsvFilter, const arr& Pinv, int verbose){ 35 | OpenGL disp; 36 | byteA img; 37 | floatA depth; 38 | bot.getImageAndDepth(img, depth, camName); 39 | arr u = getHsvBlobImageCoords(img, depth, hsvFilter, verbose-1); 40 | if(verbose>1) disp.watchImage(img, false); 41 | if(verbose>0) LOG(0) <<"dot in image coords: " <0) LOG(0) <<"dot in cam coords: " <get_X().applyOnPoint(x); 54 | if(verbose>0) LOG(0) <<"dot in world coords: " <setPosition(x); 57 | if(verbose>0 && C.view(true, "sense_HsvBlob\ngo?")=='q') return false; 58 | 59 | return true; 60 | } 61 | -------------------------------------------------------------------------------- /src/BotOp/tools.h: -------------------------------------------------------------------------------- 1 | #include "bot.h" 2 | #include 3 | 4 | struct Move_IK{ 5 | BotOp& bot; 6 | rai::Configuration& C; 7 | int askForOK; 8 | KOMO komo; 9 | arr qT; 10 | 11 | Move_IK(BotOp& _bot, rai::Configuration& _C, int _askForOK=true); 12 | 13 | KOMO& operator()(){ return komo; } 14 | 15 | bool go(); 16 | 17 | }; 18 | 19 | bool sense_HsvBlob(BotOp& bot, rai::Configuration& C, 20 | const char* camName, const char* blobName, 21 | const arr& hsvFilter, const arr& Pinv={}, int verbose=0); 22 | -------------------------------------------------------------------------------- /src/ConvexDecomposition/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | NAME = $(shell basename `pwd`) 4 | OUTPUT = lib$(NAME).so 5 | 6 | DEPEND = Core 7 | 8 | OPENCV = 1 9 | 10 | SRCS = $(shell find . -maxdepth 1 -name '*.cpp' ) 11 | OBJS = $(SRCS:%.cpp=%.o) 12 | 13 | include $(BASE)/_make/generic.mk 14 | -------------------------------------------------------------------------------- /src/ConvexDecomposition/convexDecomp2D.h: -------------------------------------------------------------------------------- 1 | #ifndef ACD2D_H 2 | #define ACD2D_H 3 | 4 | #include 5 | #include 6 | 7 | struct PolygonUtils { 8 | static bool checkIntersection(const cv::Point& p1, const cv::Point& p2, const cv::Point& p3, const cv::Point& p4); 9 | static bool checkIfLineFromContourIntersectsItself(const std::vector& contour, size_t i, size_t j); 10 | static float calcConvexityDefectOfContour(const std::vector& contour); 11 | }; 12 | 13 | struct ACD2D_kParts { 14 | ACD2D_kParts(); 15 | 16 | bool checkIfCutLineIsInside(const std::vector& contour, size_t i, size_t j); 17 | 18 | std::vector > decompose(const std::vector& contour, uint k = 2, float CDTolerance = -1); 19 | 20 | std::vector > decomposeIntoTwo(const std::vector& contour); 21 | }; 22 | 23 | #endif // ACD2D_H 24 | -------------------------------------------------------------------------------- /src/FlatVision/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | NAME = $(shell basename `pwd`) 4 | OUTPUT = lib$(NAME).so 5 | 6 | DEPEND = Core Gui Perception opencv_reg 7 | 8 | OPENCV = 1 9 | 10 | SRCS = $(shell find . -maxdepth 1 -name '*.cpp' ) 11 | OBJS = $(SRCS:%.cpp=%.o) 12 | 13 | include $(BASE)/_make/generic.mk 14 | -------------------------------------------------------------------------------- /src/FlatVision/explainBackground.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | struct ExplainBackground { 6 | //parameters 7 | int verbose=1; 8 | float threshold=.02; 9 | float farThreshold=1.1; 10 | //filter states 11 | floatA background; 12 | byteA countDeeper; 13 | floatA valueDeeper; 14 | 15 | bool computeBackground = true; 16 | 17 | void compute(byteA& pixelLabels, 18 | const byteA& cam_color, const floatA& cam_depth); 19 | 20 | void saveBackgroundModel(const char* name = "backgroundModel"); 21 | void loadBackgroundModel(const char* name = "backgroundModel"); 22 | }; 23 | -------------------------------------------------------------------------------- /src/FlatVision/explainNovels.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "helpers.h" 5 | 6 | struct ExplainNovelPercepts { 7 | //parameters 8 | int verbose=1; 9 | //internal 10 | byteA countUnexplained; 11 | //output: an array of novel flat percepts 12 | rai::Array flats; 13 | 14 | void compute(byteA& pixelLabels, 15 | const byteA& cam_color, const floatA& cam_depth); 16 | }; 17 | -------------------------------------------------------------------------------- /src/FlatVision/explainRobot.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "explainRobot.h" 5 | #include "registrationCalibration.h" 6 | 7 | void ExplainRobotPart::compute(byteA& pixelLabels, 8 | const byteA& cam_color, const floatA& cam_depth, 9 | const byteA& model_segments, const floatA& model_depth) { 10 | 11 | CHECK(label & PL_robot, "is this really a robot label?"); 12 | 13 | auto reg = registrationCalibration(cam_color, cam_depth, convert(pixelLabels==(byte)PL_unexplained), 14 | byteA(), model_depth, convert(model_segments==(byte)label), verbose); 15 | 16 | for(uint i=0;i0){ 21 | cv::Mat cv_labels = CV(pixelLabels); 22 | cv::imshow("labels after exRobot", cv_labels); 23 | cv::waitKey(1); 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /src/FlatVision/explainRobot.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "helpers.h" 5 | 6 | struct ExplainRobotPart { 7 | //parameters 8 | int verbose=1; 9 | PixelLabel label=PL_robot; 10 | //calibration output 11 | arr calib; //xy-shift (in pixels), z-shift (in meters), xy-tilt (in slope/pixel), z-tilt (in sin(phi)) 12 | 13 | void compute(byteA& pixelLabels, 14 | const byteA& cam_color, const floatA& cam_depth, 15 | const byteA& model_segments, const floatA& model_depth); 16 | }; 17 | -------------------------------------------------------------------------------- /src/FlatVision/flatVision.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | //#include 4 | #include 5 | 6 | #include "helpers.h" 7 | #include "explainBackground.h" 8 | #include "explainRobot.h" 9 | #include "explainNovels.h" 10 | #include "objectManager.h" 11 | 12 | //-- thread wrapper 13 | struct FlatVisionThread : Thread { 14 | //output 15 | // Var percepts; 16 | Var config; 17 | Var>> objects; 18 | //input 19 | Var cam_color; 20 | Var cam_depth; 21 | Var model_segments; 22 | Var model_depth; 23 | Var cam_crop; 24 | Var cam_PInv; 25 | //-- calibration variables 26 | Var armPoseCalib; //(2x6 matrix: (dx,dy,dz, rx,ry,rz) (with trans and infinitesimal rot; for both arms) 27 | //parameters 28 | int verbose=1; 29 | bool syncToConfig=true; 30 | 31 | bool updateBackground = true; 32 | 33 | //methods 34 | ExplainBackground exBackground; 35 | ExplainRobotPart exRobot; 36 | ExplainNovelPercepts exNovel; 37 | ObjectManager objectManager; 38 | 39 | FlatVisionThread(Var& _config, 40 | Var>>& _objects, 41 | Var& _color, 42 | Var& _depth, 43 | Var& _model_segments, 44 | Var _model_depth, 45 | Var& _cameraCrop, 46 | Var& _cameraPInv, 47 | Var& _armPoseCalib, 48 | int _verbose=1); 49 | ~FlatVisionThread(){ 50 | threadClose(); 51 | } 52 | void step(); 53 | }; 54 | -------------------------------------------------------------------------------- /src/FlatVision/objectManager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "helpers.h" 8 | 9 | struct ObjectManager{ 10 | uint objIdCount=0; 11 | uint flatIdCount=0; 12 | uint changeCount=0; 13 | 14 | rai::Array> flats; 15 | Var>> objects; 16 | 17 | byteA flat_segments; 18 | floatA flat_depth; 19 | floatA flat_mask; 20 | byteA flat_color; 21 | 22 | ObjectManager(Var>>& _objects); 23 | ~ObjectManager(); 24 | 25 | Object& getObject(PixelLabel pixelLabel); 26 | 27 | //-- flat world methods 28 | 29 | void renderFlatObject(int H, int W); 30 | 31 | void assignPerceptsToObjects(rai::Array& flats, 32 | const byteA& labels); 33 | 34 | void injectNovelObjects(rai::Array& flats, 35 | const byteA& labels, const byteA& cam_color, const floatA& cam_depth); 36 | 37 | void adaptFlatObjects(byteA& pixelLabels, 38 | const byteA& cam_color, const floatA& cam_depth, 39 | const uintA &cam_crop, const arr& cam_PInv, const floatA& background); 40 | 41 | void removeUnhealthyObject(rai::KinematicWorld& C); 42 | 43 | void syncWithConfig(rai::KinematicWorld& C); 44 | 45 | 46 | //-- display tools 47 | 48 | void displayLabelsAsPCL(PixelLabel label, 49 | const byteA& labels, const floatA& cam_depth, 50 | const arr& cam_pose, const arr& cam_fxycxy, 51 | rai::KinematicWorld& config); 52 | 53 | void displayLabels(const byteA& labels, 54 | const byteA& cam_color); 55 | 56 | void printObjectInfos(); 57 | 58 | }; 59 | -------------------------------------------------------------------------------- /src/FlatVision/registrationCalibration.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | struct RegReturn{ 4 | arr calib; 5 | double depthError; 6 | double matchError; 7 | }; 8 | 9 | RegReturn registrationCalibration(const byteA& cam_color, const floatA& cam_depth, const floatA& cam_mask, 10 | const byteA& model_color, const floatA& model_depth, const floatA& model_mask, 11 | bool useDepth=true, 12 | int padding=20, 13 | int verbose=1); 14 | -------------------------------------------------------------------------------- /src/FlatVision/retired/BackgroundSubtractionThread.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "BackgroundSubtractionThread.h" 6 | 7 | #if 0 //not compatible with opencv4 8 | 9 | BackgroundSubtractionThread::BackgroundSubtractionThread(Var& color, double rate) 10 | : Thread("BackgroundSubtraction", rate), color(this, color, false), rate(rate) { 11 | bgs = new cv::BackgroundSubtractorMOG2(1000.0, 0.0); 12 | threadLoop(); 13 | } 14 | 15 | BackgroundSubtractionThread::~BackgroundSubtractionThread() { 16 | threadClose(); 17 | delete bgs; 18 | } 19 | 20 | void BackgroundSubtractionThread::learnBackgroundModel(uint nFrames) { 21 | threadStop(); 22 | updateBackgroundModel = true; 23 | for(uint i = 0; i < nFrames; i++) { 24 | step(); 25 | rai::wait(rate); 26 | } 27 | updateBackgroundModel = false; 28 | threadLoop(); 29 | } 30 | 31 | void BackgroundSubtractionThread::open() {} 32 | 33 | void BackgroundSubtractionThread::close() {} 34 | 35 | void BackgroundSubtractionThread::step() { 36 | cv::Mat colorImg = conv_Arr2CvRef(color.get()()).clone(); 37 | if(!colorImg.empty()) { 38 | cv::Mat cvMask; 39 | if(updateBackgroundModel) { 40 | bgs->operator()(colorImg, cvMask); 41 | } else { 42 | bgs->operator()(colorImg, cvMask, 0.0); 43 | } 44 | cv::threshold(cvMask, cvMask, 254, 255, cv::THRESH_BINARY); 45 | 46 | if(morphologicalCleaning) { 47 | // opening and closing to get rid of small stuff 48 | cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(2*mSize+1, 2*mSize+1)); 49 | cv::morphologyEx(cvMask, cvMask, cv::MORPH_OPEN, element); 50 | cv::morphologyEx(cvMask, cvMask, cv::MORPH_CLOSE, element); 51 | } 52 | 53 | if(contourFilling) { 54 | // fill holes (hopefully) 55 | cv::Mat contourImg(cvMask.size(), CV_8UC1, cv::Scalar(0)); 56 | std::vector > contours; 57 | cv::findContours(cvMask, contours, cv::RETR_LIST, cv::CHAIN_APPROX_NONE); 58 | for(uint i = 0; i < contours.size(); i++) { 59 | cv::drawContours(contourImg, contours, i, cv::Scalar(255), -1); 60 | } 61 | mask.set()() = contourImg; 62 | } else { 63 | mask.set()() = cvMask; 64 | } 65 | } 66 | } 67 | 68 | #endif 69 | -------------------------------------------------------------------------------- /src/FlatVision/retired/BackgroundSubtractionThread.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace cv { 7 | struct Mat; 8 | struct BackgroundSubtractorMOG2; 9 | } 10 | 11 | struct BackgroundSubtractionThread : Thread { 12 | Var color; 13 | Var mask; 14 | 15 | double rate; 16 | 17 | bool updateBackgroundModel = true; 18 | 19 | bool morphologicalCleaning = true; 20 | uint mSize = 3; 21 | bool contourFilling = true; 22 | 23 | cv::BackgroundSubtractorMOG2* bgs; 24 | 25 | BackgroundSubtractionThread(Var& color, double rate = 0.01); 26 | virtual ~BackgroundSubtractionThread(); 27 | 28 | void learnBackgroundModel(uint nFrames = 100); 29 | 30 | void open(); 31 | void close(); 32 | void step(); 33 | }; 34 | 35 | -------------------------------------------------------------------------------- /src/FlatVision/retired/cv_depth_backgroundSubstraction.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | struct CV_BackgroundSubstraction_Percept{ 9 | double x=0.,y=0.; 10 | double radius=0.; 11 | arr rect; 12 | arr polygon; 13 | floatA depthRect; 14 | floatA backgroundRect; 15 | }; 16 | 17 | //-- pure algo 18 | struct CV_BackgroundSubstraction{ 19 | //parameters 20 | float threshold=.01; 21 | float depthcut = 1.05; 22 | int verbose=1; 23 | //internal 24 | byteA noSignal; 25 | byteA countDeeper; 26 | floatA valueDeeper; 27 | byteA countStable; 28 | //output 29 | floatA background; 30 | byteA mask; 31 | rai::Array CVpercepts; 32 | 33 | void compute(const byteA& _color, const floatA& _depth, const byteA& _inputMask); 34 | }; 35 | 36 | //-- thread wrapper 37 | struct CV_BackgroundSubstraction_Thread : Thread, CV_BackgroundSubstraction { 38 | //output 39 | Var percepts; 40 | //input 41 | Var color; 42 | Var depth; 43 | Var mask; 44 | Var cameraPose; 45 | Var cameraFxycxy; 46 | CV_BackgroundSubstraction_Thread(Var& _percepts, 47 | Var& _color, 48 | Var& _depth, 49 | Var& _mask, 50 | Var& _cameraPose, 51 | Var& _cameraFxycxy, int _verbose=1) 52 | : Thread("BackgroundSubstraction", -1.), 53 | percepts(this, _percepts), 54 | color(this, _color), 55 | depth(this, _depth, true), 56 | mask(this, _mask), 57 | cameraPose(this, _cameraPose), 58 | cameraFxycxy(this, _cameraFxycxy){ 59 | CV_BackgroundSubstraction::verbose = _verbose; 60 | threadOpen(); 61 | } 62 | ~CV_BackgroundSubstraction_Thread(){ 63 | threadClose(); 64 | } 65 | void step(); 66 | }; 67 | -------------------------------------------------------------------------------- /src/FlatVision/retired/explainObject.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "explainObject.h" 5 | #include "registrationCalibration.h" 6 | 7 | void ExplainObject::compute(byteA& pixelLabels, 8 | const byteA& cam_color, const floatA& cam_depth, 9 | const byteA& model_segments, const floatA& model_depth) { 10 | 11 | CHECK(label & PL_objects, "is this really an object label?"); 12 | 13 | auto reg = registrationCalibration(cam_color, cam_depth, convert(pixelLabels==(byte)PL_unexplained), 14 | byteA(), model_depth, convert(model_segments==(byte)label), verbose); 15 | 16 | for(uint i=0;i0){ 21 | cv::Mat cv_labels = conv_Arr2CvRef(pixelLabels); 22 | cv::imshow("labels after exObjects", cv_labels); 23 | cv::waitKey(1); 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /src/FlatVision/retired/explainObject.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "helpers.h" 5 | 6 | struct ExplainObject { 7 | //parameters 8 | int verbose=1; 9 | PixelLabel label=PL_objects; 10 | //calibration output 11 | arr calib; //xy-shift (in pixels), z-shift (in meters), xy-tilt (in slope/pixel), z-tilt (in sin(phi)) 12 | 13 | void compute(byteA& pixelLabels, 14 | const byteA& cam_color, const floatA& cam_depth, 15 | const byteA& model_segments, const floatA& model_depth); 16 | }; 17 | -------------------------------------------------------------------------------- /src/Franka/FrankaGripper.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace franka{ 8 | class Gripper; 9 | } 10 | 11 | //The control message send to the robot 12 | struct GripperCmdMsg { 13 | enum Command { _open, _close, _home, _done }; 14 | Command cmd=_done; 15 | double force=20; //which is 2kg 16 | double width=.05; //which is 5cm 17 | double speed=.1; 18 | }; 19 | 20 | struct FrankaGripper : rai::GripperAbstraction, Thread{ 21 | Var cmd; 22 | double maxWidth; 23 | 24 | FrankaGripper(uint whichRobot=0); 25 | ~FrankaGripper(); 26 | 27 | void homing(); 28 | 29 | void open(double width=.075, //which is 7.5cm 30 | double speed=.2); 31 | 32 | void close(double force=20, //which is 1kg 33 | double width=.05, //which is 5cm 34 | double speed=.1); 35 | 36 | double pos(); 37 | 38 | bool isDone(){ return Thread::isIdle(); } 39 | 40 | bool isGrasped(); 41 | 42 | void step(); 43 | 44 | private: 45 | std::shared_ptr frankaGripper; 46 | }; 47 | -------------------------------------------------------------------------------- /src/Franka/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | NAME = $(shell basename `pwd`) 4 | OUTPUT = lib$(NAME).so 5 | 6 | DEPEND = Core 7 | 8 | SRCS = $(shell find . -maxdepth 1 -name '*.cpp' ) 9 | OBJS = $(SRCS:%.cpp=%.o) 10 | 11 | CPATH := $(CPATH):$(HOME)/git/libfranka/include 12 | LPATH := $(LPATH):$(HOME)/git/libfranka/build 13 | LIBS += -lfranka 14 | 15 | include $(BASE)/_make/generic.mk 16 | -------------------------------------------------------------------------------- /src/Franka/franka.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | struct FrankaThread : rai::RobotAbstraction, Thread{ 10 | FrankaThread(uint robotID=0, const uintA& _qIndices={0, 1, 2, 3, 4, 5, 6}) : Thread("FrankaThread"){ init(robotID, _qIndices); } 11 | FrankaThread(uint robotID, const uintA& _qIndices, const Var& _cmd, const Var& _state) : RobotAbstraction(_cmd, _state), Thread("FrankaThread"){ init(robotID, _qIndices); } 12 | ~FrankaThread(); 13 | 14 | private: 15 | bool stop=false; //send end to libfranka 16 | bool requiresInitialization=true; //waits in constructor until first contact/initialization 17 | int robotID=0; 18 | arr Kp_freq, Kd_ratio; //read from rai.cfg 19 | arr friction; 20 | 21 | const char* ipAddress; 22 | 23 | uintA qIndices; 24 | uint qIndices_max=0; 25 | 26 | uint steps=0; 27 | ofstream dataFile; 28 | double ctrlTime=0.; 29 | 30 | void init(uint _robotID, const uintA& _qIndices); 31 | void step(); 32 | }; 33 | -------------------------------------------------------------------------------- /src/Franka/help.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | enum PixelLabel : byte { PL_unexplained=0x00, PL_nosignal=0x01, PL_noise=0x02, PL_toofar=0x03, 7 | PL_background=0x10, 8 | PL_robot=0x20, 9 | PL_novelPercepts=0x40, 10 | PL_objects=0x80, 11 | PL_closeToObject=0xc0, 12 | PL_max=0xff}; 13 | 14 | inline uintA franka_getJointIndices(const rai::Configuration& C, char L_or_R){ 15 | CHECK(C._state_indexedJoints_areGood , "need to ensure_q (indexed joints) before!"); 16 | StringA jointNames; 17 | for(uint i=1;i<=7;i++){ 18 | jointNames.append(STRING(L_or_R <<"_panda_joint" <joint->qIndex; 23 | return qIndices; 24 | } 25 | 26 | 27 | inline byteA franka_getFrameMaskMap(const rai::Configuration& K){ 28 | byteA frameMaskMap(K.frames.N); //map each frame in the image to a mask byte (here just 0 or 0xff) 29 | frameMaskMap.setZero(); 30 | for(rai::Frame *f:K.frames){ 31 | if(f->shape){ 32 | if(f->getUpwardLink()->name.startsWith("l_")){ 33 | frameMaskMap(f->ID)=PL_robot; 34 | } 35 | if(f->getUpwardLink()->name.startsWith("r_")){ 36 | frameMaskMap(f->ID)=PL_robot+1; 37 | } 38 | if(f->getUpwardLink()->name.startsWith("perc_")){ 39 | int id; 40 | f->getUpwardLink()->name >>"perc_" >>id; 41 | frameMaskMap(f->ID)=PixelLabel(PL_robot+id); 42 | } 43 | // cout <ID <<' ' <name <<' ' <ID) <shape){ 52 | if(f->getUpwardLink()->name.startsWith("l_")){ 53 | f->ats->getNew("label") = PL_robot; 54 | } 55 | if(f->getUpwardLink()->name.startsWith("r_")){ 56 | f->ats->getNew("label") = PL_robot+1; 57 | } 58 | } 59 | } 60 | } 61 | 62 | -------------------------------------------------------------------------------- /src/Franka/retired/old-franka.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | //TODO: remove this old one: 9 | struct FrankaThread : Thread{ 10 | Var ctrl; 11 | Var ctrl_state; 12 | bool stop=false, firstTime=true; 13 | arr Kp_freq, Kd_ratio; 14 | const char* ipAddress; 15 | uintA qIndices; 16 | uint qIndices_max=0; 17 | uint steps=0; 18 | 19 | FrankaThread(Var& _ctrl, Var& _state, uint whichRobot=0, const uintA& _qIndices={}); 20 | ~FrankaThread(); 21 | 22 | private: 23 | void step(); 24 | }; 25 | -------------------------------------------------------------------------------- /src/IPC/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | NAME = $(shell basename `pwd`) 3 | OUTPUT = lib$(NAME).so 4 | 5 | DEPEND = Core extern_lcm 6 | DEPEND_UBUNTU = libglib2.0-dev 7 | 8 | SRCS = $(shell find . -maxdepth 1 -name '*.cpp' ) 9 | OBJS = $(SRCS:%.cpp=%.o) 10 | 11 | include $(BASE)/_make/generic.mk 12 | -------------------------------------------------------------------------------- /src/IPC/ipc.cpp: -------------------------------------------------------------------------------- 1 | #include "ipc.h" 2 | 3 | #include "lcm/lcm-cpp.hpp" 4 | #include "lcm/lcm.h" 5 | 6 | struct LCM_Interface : Thread{ 7 | lcm::LCM lcm; 8 | 9 | LCM_Interface() : Thread("LCM_Spinner", -1.) { threadLoop(); } 10 | ~LCM_Interface() { threadClose(); } 11 | 12 | void step(){ 13 | int ret = lcm.handle(); 14 | CHECK(!ret, "LCM step failed"); 15 | } 16 | 17 | }; 18 | 19 | Singleton LCM; 20 | 21 | void __lcm_publish(const char* message_name, char* data, uint32_t data_size){ 22 | int ret = LCM()->lcm.publish(message_name, data, data_size); 23 | if(ret) LOG(-1) <<"publishing '" <lcm.subscribeFunction(message_name, callback, var); 30 | } 31 | -------------------------------------------------------------------------------- /src/IPC/ipc.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | template void lcm_publish(Var& x, const char* message_name); 4 | template void lcm_subscribe(Var& x, const char* message_name); 5 | 6 | struct CompoundSerializable : Serializable{ 7 | std::vector objs; 8 | CompoundSerializable(std::initializer_list _objs) : objs(_objs){} 9 | 10 | virtual uint serial_size(){ 11 | uint s=0; 12 | for(auto* o:objs) s += o->serial_size(); 13 | return s; 14 | } 15 | 16 | virtual uint serial_encode(char* data, uint data_size){ 17 | uint s=0; 18 | for(auto* o:objs){ 19 | CHECK_GE(data_size, s, ""); 20 | s += o->serial_encode(data + s, data_size - s); 21 | } 22 | CHECK_GE(data_size, s, ""); 23 | return s; 24 | } 25 | 26 | virtual uint serial_decode(char* data, uint data_size){ 27 | uint s=0; 28 | for(auto* o:objs){ 29 | CHECK_GE(data_size, s, ""); 30 | s += o->serial_decode(data + s, data_size - s); 31 | } 32 | CHECK_GE(data_size, s, ""); 33 | return s; 34 | } 35 | }; 36 | 37 | #include "ipc.tpp" 38 | -------------------------------------------------------------------------------- /src/IPC/ipc.tpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "lcm/lcm-cpp.hpp" 4 | 5 | void __lcm_publish(const char *message_name, char* data, uint size); 6 | void __lcm_subscribe(const char *message_name, void (*callback)(const lcm::ReceiveBuffer*, const std::string&, Var_base*), Var_base* var); 7 | 8 | template 9 | void subscribeCallback(const lcm::ReceiveBuffer *rbuf, const std::string &channel, Var_base* var){ 10 | Var_data* x = dynamic_cast*>(var); 11 | CHECK(x, ""); 12 | x->writeAccess(); 13 | x->data.serial_decode((char*)rbuf->data, rbuf->data_size); 14 | x->deAccess(); 15 | } 16 | 17 | template 18 | void publishCallback(Var_base* var){ 19 | Var_data* x = dynamic_cast*>(var); 20 | CHECK(x, ""); 21 | std::vector buf(x->data.serial_size()); 22 | x->data.serial_encode(buf.data(), buf.size()); 23 | __lcm_publish(x->name.p, buf.data(), buf.size()); 24 | } 25 | 26 | template 27 | void lcm_publish(Var& x, const char* message_name){ 28 | x.data->name = message_name; 29 | x.addCallback(publishCallback); 30 | } 31 | 32 | template 33 | void lcm_subscribe(Var& x, const char* message_name){ 34 | __lcm_subscribe(message_name, subscribeCallback, x.data.get()); 35 | } 36 | 37 | 38 | -------------------------------------------------------------------------------- /src/IPC/lcm/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../../rai 2 | NAME = $(shell basename `pwd`) 3 | OUTPUT = libextern_$(NAME).so 4 | 5 | CFLAGS += -O3 -fPIC -w `pkg-config glib-2.0 --cflags` 6 | LIBS += `pkg-config glib-2.0 --libs` 7 | 8 | SRCS = $(shell find . -maxdepth 2 -name '*.c' ) 9 | OBJS = $(SRCS:%.c=%.o) 10 | 11 | include $(BASE)/_make/generic.mk 12 | -------------------------------------------------------------------------------- /src/IPC/lcm/ioutils.h: -------------------------------------------------------------------------------- 1 | #ifndef __LCM_IOUTILS_H__ 2 | #define __LCM_IOUTILS_H__ 3 | 4 | #include 5 | #include 6 | #ifndef WIN32 7 | #include 8 | #else 9 | #include 10 | #endif 11 | 12 | #ifdef __cplusplus 13 | extern "C" { 14 | #endif 15 | 16 | static inline int fwrite32(FILE *f, int32_t v) 17 | { 18 | v = htonl(v); 19 | if (fwrite(&v, 4, 1, f) == 1) 20 | return 0; 21 | else 22 | return -1; 23 | } 24 | 25 | static inline int fwrite64(FILE *f, int64_t v64) 26 | { 27 | // See Section 5.8 paragraph 3 of the standard 28 | // http://open-std.org/JTC1/SC22/WG21/docs/papers/2015/n4527.pdf 29 | // use uint for shifting instead if int 30 | int32_t v = ((uint64_t) v64) >> 32; 31 | if (0 != fwrite32(f, v)) 32 | return -1; 33 | v = v64 & 0xffffffff; 34 | return fwrite32(f, v); 35 | } 36 | 37 | static inline int fread32(FILE *f, int32_t *v32) 38 | { 39 | int32_t v; 40 | 41 | if (fread(&v, 4, 1, f) != 1) 42 | return -1; 43 | 44 | *v32 = ntohl(v); 45 | 46 | return 0; 47 | } 48 | 49 | static inline int fread64(FILE *f, int64_t *v64) 50 | { 51 | int32_t v1, v2; 52 | 53 | if (fread32(f, &v1)) 54 | return -1; 55 | 56 | if (fread32(f, &v2)) 57 | return -1; 58 | 59 | // See Section 5.8 paragraph 3 of the standard 60 | // http://open-std.org/JTC1/SC22/WG21/docs/papers/2015/n4527.pdf 61 | // use uint for shifting instead if int 62 | *v64 = (int64_t)(((uint64_t) v1) << 32) | (((int64_t) v2) & 0xffffffff); 63 | 64 | return 0; 65 | } 66 | 67 | #ifdef __cplusplus 68 | } 69 | #endif 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /src/IPC/lcm/lcm_export.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef LCM_EXPORT_H 3 | #define LCM_EXPORT_H 4 | 5 | #ifdef LCM_STATIC 6 | # define LCM_EXPORT 7 | # define LCM_NO_EXPORT 8 | #else 9 | # ifndef LCM_EXPORT 10 | # ifdef lcm_EXPORTS 11 | /* We are building this library */ 12 | # define LCM_EXPORT __attribute__((visibility("default"))) 13 | # else 14 | /* We are using this library */ 15 | # define LCM_EXPORT __attribute__((visibility("default"))) 16 | # endif 17 | # endif 18 | 19 | # ifndef LCM_NO_EXPORT 20 | # define LCM_NO_EXPORT __attribute__((visibility("hidden"))) 21 | # endif 22 | #endif 23 | 24 | #ifndef LCM_DEPRECATED 25 | # define LCM_DEPRECATED __attribute__ ((__deprecated__)) 26 | #endif 27 | 28 | #ifndef LCM_DEPRECATED_EXPORT 29 | # define LCM_DEPRECATED_EXPORT LCM_EXPORT LCM_DEPRECATED 30 | #endif 31 | 32 | #ifndef LCM_DEPRECATED_NO_EXPORT 33 | # define LCM_DEPRECATED_NO_EXPORT LCM_NO_EXPORT LCM_DEPRECATED 34 | #endif 35 | 36 | #define DEFINE_NO_DEPRECATED 0 37 | #if DEFINE_NO_DEPRECATED 38 | # define LCM_NO_DEPRECATED 39 | #endif 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /src/IPC/lcm/lcm_internal.h: -------------------------------------------------------------------------------- 1 | #ifndef __LCM_INTERNAL_H__ 2 | #define __LCM_INTERNAL_H__ 3 | 4 | #include 5 | #include "lcm.h" 6 | 7 | // GRegex was new in GLib 2.14.0 8 | #if GLIB_CHECK_VERSION(2, 14, 0) 9 | #else 10 | #error "LCM requires a glib version >= 2.14.0" 11 | #endif 12 | 13 | #ifdef WIN32 14 | #include 15 | #include "windows/WinPorting.h" 16 | #else 17 | // in POSIX systems, the normal read/write/close/pipe functions are fine for 18 | // inter-thread signaling via file descriptors. 19 | #include 20 | #define lcm_internal_pipe_write write 21 | #define lcm_internal_pipe_read read 22 | #define lcm_internal_pipe_close close 23 | #define lcm_internal_pipe_create pipe 24 | #endif 25 | 26 | typedef struct _lcm_provider_t lcm_provider_t; 27 | typedef struct _lcm_provider_info_t lcm_provider_info_t; 28 | typedef struct _lcm_provider_vtable_t lcm_provider_vtable_t; 29 | 30 | struct _lcm_provider_info_t { 31 | char *name; 32 | lcm_provider_vtable_t *vtable; 33 | }; 34 | 35 | struct _lcm_provider_vtable_t { 36 | lcm_provider_t *(*create)(lcm_t *, const char *target, const GHashTable *args); 37 | void (*destroy)(lcm_provider_t *); 38 | int (*subscribe)(lcm_provider_t *, const char *channel); 39 | int (*unsubscribe)(lcm_provider_t *, const char *channel); 40 | int (*publish)(lcm_provider_t *, const char *, const void *, unsigned int); 41 | int (*handle)(lcm_provider_t *); 42 | int (*get_fileno)(lcm_provider_t *); 43 | }; 44 | 45 | int lcm_parse_url(const char *url, char **provider, char **target, GHashTable *args); 46 | 47 | /** 48 | * Try to enqueue a message. This may fail if there are no subscribers, or if 49 | * all the subscribers' queues are full. The actual message contents are not 50 | * enqueued here, only a placeholder for the message. 51 | */ 52 | int lcm_try_enqueue_message(lcm_t *lcm, const char *channel); 53 | 54 | int lcm_has_handlers(lcm_t *lcm, const char *channel); 55 | 56 | int lcm_dispatch_handlers(lcm_t *lcm, lcm_recv_buf_t *buf, const char *channel); 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /src/IPC/lcm/lcm_version.h: -------------------------------------------------------------------------------- 1 | /// LCM release major version - the X in version X.Y.Z 2 | #define LCM_VERSION_MAJOR 1 3 | 4 | /// LCM release minor version - the Y in version X.Y.Z 5 | #define LCM_VERSION_MINOR 4 6 | 7 | /// LCM release patch version - the Z in version X.Y.Z 8 | #define LCM_VERSION_PATCH 0 9 | 10 | /// LCM ABI version 11 | #define LCM_ABI_VERSION 1 12 | 13 | // Old symbols provided for compatibility 14 | #define LCM_MAJOR_VERSION LCM_VERSION_MAJOR 15 | #define LCM_MINOR_VERSION LCM_VERSION_MINOR 16 | #define LCM_MICRO_VERSION LCM_VERSION_PATCH 17 | -------------------------------------------------------------------------------- /src/IPC/lcm/lcmtypes/channel_port_map_update_t.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Generated by running lcm-gen -c --c-no-pubsub channel_port_mapping.lcm 3 | * 4 | * and then modified by hand to replace 5 | * #include 6 | * with 7 | * #include "../lcm_coretypes.h" 8 | **/ 9 | 10 | #include 11 | #include 12 | #include "../lcm_coretypes.h" 13 | 14 | #ifndef _channel_port_map_update_t_h 15 | #define _channel_port_map_update_t_h 16 | 17 | #ifdef __cplusplus 18 | extern "C" { 19 | #endif 20 | 21 | #include "channel_to_port_t.h" 22 | typedef struct _channel_port_map_update_t channel_port_map_update_t; 23 | struct _channel_port_map_update_t { 24 | int16_t num_ports; 25 | int16_t num_channels; 26 | channel_to_port_t *mapping; 27 | }; 28 | 29 | channel_port_map_update_t *channel_port_map_update_t_copy(const channel_port_map_update_t *p); 30 | void channel_port_map_update_t_destroy(channel_port_map_update_t *p); 31 | 32 | int channel_port_map_update_t_encode(void *buf, int offset, int maxlen, 33 | const channel_port_map_update_t *p); 34 | int channel_port_map_update_t_decode(const void *buf, int offset, int maxlen, 35 | channel_port_map_update_t *p); 36 | int channel_port_map_update_t_decode_cleanup(channel_port_map_update_t *p); 37 | int channel_port_map_update_t_encoded_size(const channel_port_map_update_t *p); 38 | 39 | // LCM support functions. Users should not call these 40 | int64_t __channel_port_map_update_t_get_hash(void); 41 | int64_t __channel_port_map_update_t_hash_recursive(const __lcm_hash_ptr *p); 42 | int __channel_port_map_update_t_encode_array(void *buf, int offset, int maxlen, 43 | const channel_port_map_update_t *p, int elements); 44 | int __channel_port_map_update_t_decode_array(const void *buf, int offset, int maxlen, 45 | channel_port_map_update_t *p, int elements); 46 | int __channel_port_map_update_t_decode_array_cleanup(channel_port_map_update_t *p, int elements); 47 | int __channel_port_map_update_t_encoded_array_size(const channel_port_map_update_t *p, 48 | int elements); 49 | int __channel_port_map_update_t_clone_array(const channel_port_map_update_t *p, 50 | channel_port_map_update_t *q, int elements); 51 | 52 | #ifdef __cplusplus 53 | } 54 | #endif 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /src/IPC/lcm/lcmtypes/channel_port_mapping.lcm: -------------------------------------------------------------------------------- 1 | // Definitions of the structures used to communicate between nodes. 2 | // 3 | // We also check in the autogenerated c bindings so that we don't need for lcm-gen 4 | // to be working in order to compile. 5 | // 6 | // The .c and .h files were generated by running 7 | // $ lcm-gen -c --c-no-pubsub channel_port_mapping.lcm 8 | // and then modified by hand to replace: 9 | // #include 10 | // with 11 | // #include "../lcm_coretypes.h" 12 | 13 | 14 | struct channel_to_port_t 15 | { 16 | string channel; 17 | int16_t port; //ports are uint16_t 18 | } 19 | 20 | struct channel_port_map_update_t 21 | { 22 | int16_t num_ports; // size of the port range for the mappings 23 | 24 | int16_t num_channels; 25 | channel_to_port_t mapping[num_channels]; 26 | } 27 | -------------------------------------------------------------------------------- /src/IPC/lcm/lcmtypes/channel_to_port_t.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Generated by running lcm-gen -c --c-no-pubsub channel_port_mapping.lcm 3 | * 4 | * and then modified by hand to replace 5 | * #include 6 | * with 7 | * #include "../lcm_coretypes.h" 8 | **/ 9 | 10 | #include 11 | #include 12 | #include "../lcm_coretypes.h" 13 | 14 | #ifndef _channel_to_port_t_h 15 | #define _channel_to_port_t_h 16 | 17 | #ifdef __cplusplus 18 | extern "C" { 19 | #endif 20 | 21 | typedef struct _channel_to_port_t channel_to_port_t; 22 | struct _channel_to_port_t { 23 | char *channel; 24 | int16_t port; 25 | }; 26 | 27 | channel_to_port_t *channel_to_port_t_copy(const channel_to_port_t *p); 28 | void channel_to_port_t_destroy(channel_to_port_t *p); 29 | 30 | int channel_to_port_t_encode(void *buf, int offset, int maxlen, const channel_to_port_t *p); 31 | int channel_to_port_t_decode(const void *buf, int offset, int maxlen, channel_to_port_t *p); 32 | int channel_to_port_t_decode_cleanup(channel_to_port_t *p); 33 | int channel_to_port_t_encoded_size(const channel_to_port_t *p); 34 | 35 | // LCM support functions. Users should not call these 36 | int64_t __channel_to_port_t_get_hash(void); 37 | int64_t __channel_to_port_t_hash_recursive(const __lcm_hash_ptr *p); 38 | int __channel_to_port_t_encode_array(void *buf, int offset, int maxlen, const channel_to_port_t *p, 39 | int elements); 40 | int __channel_to_port_t_decode_array(const void *buf, int offset, int maxlen, channel_to_port_t *p, 41 | int elements); 42 | int __channel_to_port_t_decode_array_cleanup(channel_to_port_t *p, int elements); 43 | int __channel_to_port_t_encoded_array_size(const channel_to_port_t *p, int elements); 44 | int __channel_to_port_t_clone_array(const channel_to_port_t *p, channel_to_port_t *q, int elements); 45 | 46 | #ifdef __cplusplus 47 | } 48 | #endif 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /src/IPC/lcm/ringbuffer.h: -------------------------------------------------------------------------------- 1 | #ifndef __lcm_ringbuffer_h__ 2 | #define __lcm_ringbuffer_h__ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include 9 | 10 | typedef struct _lcm_ringbuf lcm_ringbuf_t; 11 | 12 | lcm_ringbuf_t *lcm_ringbuf_new(unsigned int ring_size); 13 | void lcm_ringbuf_free(lcm_ringbuf_t *ring); 14 | 15 | /* 16 | * Allocates a variable-sized chunk of the ring buffer for use by the 17 | * application. Returns the pointer to the available chunk. 18 | */ 19 | char *lcm_ringbuf_alloc(lcm_ringbuf_t *ring, unsigned int len); 20 | 21 | /* 22 | * resizes the most recently allocated chunk of the ring buffer. The newly 23 | * requested size must be smaller than the original chunk size. 24 | */ 25 | void lcm_ringbuf_shrink_last(lcm_ringbuf_t *ring, const char *buf, unsigned int len); 26 | 27 | unsigned int lcm_ringbuf_capacity(lcm_ringbuf_t *ring); 28 | 29 | unsigned int lcm_ringbuf_used(lcm_ringbuf_t *ring); 30 | 31 | /* 32 | * Releases a previously-allocated chunk of the ring buffer. Only the most 33 | * recently allocated, or the least recently allocated chunk can be released. 34 | */ 35 | void lcm_ringbuf_dealloc(lcm_ringbuf_t *ring, char *buf); 36 | 37 | #ifdef __cplusplus 38 | } 39 | #endif 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /src/LGPop/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = .. 3 | NAME = $(shell basename `pwd`) 4 | OUTPUT = lib$(NAME).so 5 | 6 | DEPEND = Core Perception Franka RealSense FlatVision NewControl CameraCalibration RosCom opencv_reg 7 | OPENCV = 1 8 | 9 | PYBIND = 0 10 | 11 | 12 | SRCS = $(shell find . -maxdepth 1 -name '*.cpp' ) 13 | OBJS = $(SRCS:%.cpp=%.o) 14 | 15 | include $(BASE)/_make/generic.mk 16 | -------------------------------------------------------------------------------- /src/LGPop/opy.cpp: -------------------------------------------------------------------------------- 1 | #ifdef RAI_PYBIND 2 | 3 | #include "opy.h" 4 | #include "lgpop.h" 5 | 6 | void RyLGPop::runRobotControllers(LGPop::OpMode opMode){ 7 | self->runRobotControllers(opMode); 8 | } 9 | 10 | void RyLGPop::runTaskController(int verbose){ 11 | self->runTaskController(verbose); 12 | } 13 | 14 | void RyLGPop::runCamera(int verbose){ 15 | self->runCamera(verbose); 16 | } 17 | 18 | void RyLGPop::runPerception(int verbose){ 19 | self->runPerception(verbose); 20 | } 21 | 22 | void RyLGPop::runCalibration(){ 23 | self->runCalibration(); 24 | } 25 | 26 | void RyLGPop::reportCycleTimes(){ 27 | self->reportCycleTimes(); 28 | } 29 | 30 | void RyLGPop::updateArmPoseCalibInModel(){ 31 | self->updateArmPoseCalibInModel(); 32 | } 33 | 34 | void RyLGPop::sim_addRandomBox(const char* name){ 35 | self->sim_addRandomBox(name); 36 | } 37 | 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | namespace py = pybind11; 44 | 45 | 46 | #define METHOD(method) .def(#method, &RyLGPop::method) 47 | #define METHOD_set(method) .def(#method, [](ry::Config& self) { self.set()->method(); } ) 48 | #define METHOD_set1(method, arg1) .def(#method, [](ry::Config& self) { self.set()->method(arg1); } ) 49 | 50 | 51 | PYBIND11_MODULE(libLGPop, m) { 52 | 53 | py::class_(m, "LGPop") 54 | .def(py::init()) 55 | METHOD(runRobotControllers) 56 | METHOD(runTaskController) 57 | METHOD(runCamera) 58 | METHOD(runPerception) 59 | METHOD(runCalibration) 60 | METHOD(reportCycleTimes) 61 | METHOD(updateArmPoseCalibInModel) 62 | METHOD(sim_addRandomBox) 63 | ; 64 | 65 | } 66 | 67 | #endif 68 | 69 | 70 | -------------------------------------------------------------------------------- /src/LGPop/opy.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "lgpop.h" 4 | 5 | struct RyLGPop{ 6 | std::shared_ptr self; 7 | 8 | RyLGPop(LGPop::OpMode opMode=LGPop::RealMode){ 9 | self = make_shared(opMode); 10 | } 11 | 12 | void runRobotControllers(LGPop::OpMode opMode=LGPop::RealMode); 13 | void runTaskController(int verbose=0); 14 | void runCamera(int verbose=0); 15 | void runPerception(int verbose=0); 16 | void runCalibration(); 17 | 18 | void reportCycleTimes(); 19 | 20 | void updateArmPoseCalibInModel(); 21 | 22 | void sim_addRandomBox(const char* name="randomBox"); 23 | }; 24 | -------------------------------------------------------------------------------- /src/MarkerVision/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | NAME = $(shell basename `pwd`) 4 | OUTPUT = lib$(NAME).so 5 | 6 | DEPEND = Core Gui Perception KOMO 7 | 8 | OPENCV = 1 9 | 10 | SRCS = $(shell find . -maxdepth 1 -name '*.cpp' ) 11 | OBJS = $(SRCS:%.cpp=%.o) 12 | 13 | include $(BASE)/_make/generic.mk 14 | -------------------------------------------------------------------------------- /src/MarkerVision/cvTools.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | void makeHomogeneousImageCoordinate(arr& u); 4 | 5 | void decomposeInvProjectionMatrix(arr& K, arr& R, arr& t, const arr& P); 6 | 7 | arr getHsvBlobImageCoords(byteA& _rgb, floatA& _depth, const arr& hsvFilter, int verbose=0, arr& histograms=NoArr); 8 | 9 | struct CameraCalibrationHSVGui { 10 | static const char* window_detection_name; 11 | int low_H=0, low_S=0, low_V=0; 12 | int high_H=180, high_S=255, high_V=255; 13 | 14 | CameraCalibrationHSVGui(); 15 | 16 | arr getHSV(); 17 | 18 | protected: 19 | static void on_low_H_thresh_trackbar(int, void* self); 20 | static void on_high_H_thresh_trackbar(int, void* self); 21 | static void on_low_S_thresh_trackbar(int, void* self); 22 | static void on_high_S_thresh_trackbar(int, void* self); 23 | static void on_low_V_thresh_trackbar(int, void* self); 24 | static void on_high_V_thresh_trackbar(int, void* self); 25 | }; 26 | 27 | -------------------------------------------------------------------------------- /src/MarkerVision/markerVision.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/src/MarkerVision/markerVision.cpp -------------------------------------------------------------------------------- /src/MarkerVision/markerVision.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarcToussaint/botop/8d99476cd552c2f1385cfcfa023e2f8619839f6b/src/MarkerVision/markerVision.h -------------------------------------------------------------------------------- /src/NewControl/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | NAME = $(shell basename `pwd`) 4 | OUTPUT = lib$(NAME).so 5 | 6 | DEPEND = Core Kin 7 | 8 | SRCS = $(shell find . -maxdepth 1 -name '*.cpp' ) 9 | OBJS = $(SRCS:%.cpp=%.o) 10 | 11 | include $(BASE)/_make/generic.mk 12 | -------------------------------------------------------------------------------- /src/NewControl/ctrlMsgs.h: -------------------------------------------------------------------------------- 1 | /* ------------------------------------------------------------------ 2 | Copyright (c) 2017 Marc Toussaint 3 | email: marc.toussaint@informatik.uni-stuttgart.de 4 | 5 | This code is distributed under the MIT License. 6 | Please see /LICENSE for details. 7 | -------------------------------------------------------------- */ 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | enum class ControlType { configRefs, projectedAcc }; 14 | 15 | //The control message send to the robot 16 | struct CtrlCmdMsg { 17 | ControlType controlType=ControlType::configRefs; 18 | arr qRef, qDotRef; // joint space references 19 | arr qDDotRef; // joint acceleration feedforward reference 20 | arr u_b; // open-loop/feed-forward torque term 21 | arr Kp, Kd; // gain matrices 22 | arr P_compliance; 23 | }; 24 | 25 | // The state message comming back from the robot 26 | struct CtrlStateMsg { 27 | arr q, qDot; // actual joint state 28 | arr tauExternal; // external torques 29 | }; 30 | -------------------------------------------------------------------------------- /src/Omnibase/SimplexMotion.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define RAI_2PI 6.283195307179587 4 | 5 | //=========================================================================== 6 | 7 | //user interface 8 | struct SimplexMotion{ 9 | struct SimplexMotion_Communication* com=0; 10 | double maxTorque = 2.; 11 | 12 | SimplexMotion(const char* devPath = "/dev/hidraw0", unsigned short vendor_id = 0x04d8, unsigned short product_id = 0xf79a); 13 | ~SimplexMotion(); 14 | 15 | const char* getModelName(); 16 | const char* getSerialNumber(); 17 | int getAddress(); 18 | 19 | double getVoltage(); 20 | double getMotorTemperature(); 21 | double getMotorPosition(); 22 | double getMotorSpeed(); 23 | double getMotorTorque(); 24 | 25 | void setSpeedFiltering(int filter); 26 | void setMaxTorque(double maxTorqueInNm =1.); 27 | 28 | //using the ramp & PID regulator that maps pos/vel to torques 29 | void setPID(int Kp, int Ki, int Kd, int KiLimit, int KdDelay, int Friction); 30 | void setRamps(double maxSpeed, double maxAcc); 31 | void setIntertia(int mass); 32 | double getPIDoutput(); //torque reference 33 | 34 | void setMode(int mode); 35 | void setTarget(int target); 36 | 37 | void runOff(){ setMode(0); } 38 | void runReset(){ setMode(1); } 39 | void runStop(){ setMode(5); } 40 | void runCoggingCalibration(){ setMode(110); } 41 | void runSpeed(double speed){ setTarget(256.*speed/RAI_2PI); setMode(33); } //using ramp & PID; activates SpeedRamp control mode 42 | void runPosition(double position){ setTarget(4096.*position/RAI_2PI); setMode(21); } //using ramps & PIDactivates PositionRamp control mode 43 | void runTorque(double torque){ setTarget(torque/maxTorque*32767); setMode(40); } 44 | 45 | }; 46 | 47 | //=========================================================================== 48 | -------------------------------------------------------------------------------- /src/Omnibase/omnibase.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | struct OmnibaseController; 9 | 10 | struct OmnibaseThread : rai::RobotAbstraction, Thread { 11 | OmnibaseThread(uint robotID, const uintA& _qIndices, const Var& _cmd, const Var& _state) : RobotAbstraction(_cmd, _state), Thread("OmnibaseThread", .01){ init(robotID, _qIndices); } 12 | ~OmnibaseThread(); 13 | 14 | private: 15 | int robotID=0; 16 | arr Kp_freq, Kd_ratio; //read from rai.cfg 17 | rai::String address; 18 | 19 | uintA qIndices; 20 | uint qIndices_max=0; 21 | 22 | uint steps=0; 23 | ofstream dataFile; 24 | double ctrlTime=0.; 25 | 26 | std::shared_ptr robot; 27 | 28 | 29 | void init(uint _robotID, const uintA& _qIndices); 30 | void open(); 31 | void step(); 32 | void close(); 33 | }; 34 | -------------------------------------------------------------------------------- /src/OptiTrack/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | NAME = $(shell basename `pwd`) 4 | OUTPUT = lib$(NAME).so 5 | 6 | EIGEN = 1 7 | 8 | DEPEND = Core 9 | 10 | CXX_FLAGS = -DENABLE_OPTITRACK 11 | 12 | SRCS = $(shell find . -maxdepth 1 -name '*.cpp' ) 13 | OBJS = $(SRCS:%.cpp=%.o) 14 | 15 | include $(BASE)/_make/generic.mk 16 | -------------------------------------------------------------------------------- /src/OptiTrack/motioncapture.cpp: -------------------------------------------------------------------------------- 1 | /*** 2 | * TAKEN FROM https://github.com/IMRCLab/libmotioncapture 3 | * Aug 26, 2021 4 | */ 5 | 6 | #include "motioncapture.h" 7 | 8 | #define ENABLE_OPTITRACK 9 | 10 | #ifdef ENABLE_VICON 11 | #include "libmotioncapture/vicon.h" 12 | #endif 13 | #ifdef ENABLE_OPTITRACK 14 | #include "motioncapture_optitrack.h" 15 | #endif 16 | #ifdef ENABLE_QUALISYS 17 | #include "libmotioncapture/qualisys.h" 18 | #endif 19 | #ifdef ENABLE_VRPN 20 | #include "libmotioncapture/vrpn.h" 21 | #endif 22 | 23 | namespace libmotioncapture { 24 | 25 | const char *version_string = "1.0a1"; 26 | 27 | const char *version() 28 | { 29 | return version_string; 30 | } 31 | 32 | RigidBody MotionCapture::rigidBodyByName( 33 | const std::string& name) const 34 | { 35 | const auto& obj = rigidBodies(); 36 | const auto iter = obj.find(name); 37 | if (iter != obj.end()) { 38 | return iter->second; 39 | } 40 | throw std::runtime_error("Rigid body not found!"); 41 | } 42 | 43 | MotionCapture* MotionCapture::connect( 44 | const std::string& type, 45 | const std::string& hostname) 46 | { 47 | MotionCapture* mocap = nullptr; 48 | 49 | if (false) 50 | { 51 | } 52 | #ifdef ENABLE_VICON 53 | else if (type == "vicon") 54 | { 55 | mocap = new libmotioncapture::MotionCaptureVicon( 56 | hostname, 57 | /*enable_objects*/ true, 58 | /*enable_pointclout*/ true); 59 | } 60 | #endif 61 | #ifdef ENABLE_OPTITRACK 62 | else if (type == "optitrack") 63 | { 64 | mocap = new libmotioncapture::MotionCaptureOptitrack( 65 | hostname); 66 | } 67 | #endif 68 | #ifdef ENABLE_QUALISYS 69 | else if (type == "qualisys") 70 | { 71 | mocap = new libmotioncapture::MotionCaptureQualisys( 72 | hostname, 73 | /*port*/ 22222, 74 | /*enable_objects*/ true, 75 | /*enable_pointclout*/ true); 76 | } 77 | #endif 78 | #ifdef ENABLE_VRPN 79 | else if (type == "vrpn") 80 | { 81 | mocap = new libmotioncapture::MotionCaptureVrpn( 82 | hostname); 83 | } 84 | #endif 85 | else 86 | { 87 | throw std::runtime_error("Unknown motion capture type!"); 88 | } 89 | 90 | return mocap; 91 | } 92 | 93 | } 94 | -------------------------------------------------------------------------------- /src/OptiTrack/motioncapture_optitrack.h: -------------------------------------------------------------------------------- 1 | /*** 2 | * TAKEN FROM https://github.com/IMRCLab/libmotioncapture 3 | * Aug 26, 2021 4 | */ 5 | 6 | #pragma once 7 | #include "motioncapture.h" 8 | 9 | namespace libmotioncapture { 10 | class MotionCaptureOptitrackImpl; 11 | 12 | class MotionCaptureOptitrack : public MotionCapture{ 13 | public: 14 | MotionCaptureOptitrack( 15 | const std::string& hostname); 16 | 17 | virtual ~MotionCaptureOptitrack(); 18 | 19 | const std::string& version() const; 20 | 21 | // implementations for MotionCapture interface 22 | virtual void waitForNextFrame(); 23 | virtual const std::map& rigidBodies() const; 24 | virtual const PointCloud& pointCloud() const; 25 | 26 | virtual bool supportsRigidBodyTracking() const 27 | { 28 | return true; 29 | } 30 | virtual bool supportsPointCloud() const 31 | { 32 | return true; 33 | } 34 | 35 | private: 36 | MotionCaptureOptitrackImpl * pImpl; 37 | }; 38 | } 39 | 40 | -------------------------------------------------------------------------------- /src/OptiTrack/optitrack.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace libmotioncapture{ class MotionCapture; } 7 | 8 | namespace rai{ 9 | 10 | struct OptiTrack : Thread { 11 | libmotioncapture::MotionCapture *mocap = 0; 12 | 13 | RAI_PARAM("optitrack/", double, filter, .9) 14 | 15 | OptiTrack(); 16 | ~OptiTrack(); 17 | 18 | void pull(rai::Configuration& C); 19 | 20 | void step(); 21 | 22 | private: 23 | std::mutex mux; 24 | std::map poses; 25 | }; 26 | 27 | } //namespace 28 | -------------------------------------------------------------------------------- /src/RealSense/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | NAME = $(shell basename `pwd`) 4 | OUTPUT = lib$(NAME).so 5 | 6 | DEPEND = Core 7 | #to compile realsense: 8 | DEPEND_UBUNTU = libusb-1.0-0-dev libglfw3-dev libgtk-3-dev 9 | 10 | CXXFLAGS += -DRAI_REALSENSE `pkg-config --cflags realsense2` 11 | LIBS += `pkg-config --libs realsense2` 12 | 13 | SRCS = $(shell find . -maxdepth 1 -name '*.cpp' ) 14 | OBJS = $(SRCS:%.cpp=%.o) 15 | 16 | include $(BASE)/_make/generic.mk 17 | -------------------------------------------------------------------------------- /src/RealSense/MultiRealSenseThread.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace rs2 { 8 | struct config; 9 | struct pipeline; 10 | struct align; 11 | } 12 | 13 | namespace rai { 14 | namespace realsense { 15 | 16 | extern std::unordered_map cameraMapping; 17 | 18 | struct RealSenseCamera { 19 | std::string cameraName; 20 | bool captureColor; 21 | bool captureDepth; 22 | std::string serialNumber; 23 | std::shared_ptr cfg; 24 | std::shared_ptr pipe; 25 | std::shared_ptr align; 26 | float depth_scale; 27 | arr fxycxy, color_fxycxy, depth_fxycxy; 28 | 29 | RealSenseCamera(std::string cameraName, bool captureColor, bool captureDepth); 30 | }; 31 | 32 | struct MultiRealSenseThread : Thread { 33 | std::vector cameraNames; 34 | Var> color; 35 | Var> depth; 36 | bool captureColor; 37 | bool captureDepth; 38 | 39 | std::vector cameras; 40 | 41 | MultiRealSenseThread(const std::vector cameraNames, 42 | const Var>& color, 43 | const Var>& depth, 44 | bool captureColor, bool captureDepth); 45 | ~MultiRealSenseThread(); 46 | 47 | uint getNumberOfCameras(); 48 | 49 | void open(); 50 | void close(); 51 | void step(); 52 | }; 53 | 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /src/RealSense/RealSenseThread.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace rs2 { class pipeline; } 8 | 9 | struct RealSenseThread : Thread, rai::CameraAbstraction { 10 | struct sRealSenseThread *s=0; 11 | Var image; 12 | Var depth; 13 | arr fxycxy, color_fxycxy, depth_fxycxy; 14 | 15 | RealSenseThread(const char *_name); 16 | ~RealSenseThread(); 17 | 18 | virtual void getImageAndDepth(byteA& _image, floatA& _depth); 19 | arr getFxycxy(){ return fxycxy; } 20 | 21 | protected: 22 | void open(); 23 | void close(); 24 | void step(); 25 | }; 26 | -------------------------------------------------------------------------------- /src/RealSense/utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifdef RAI_REALSENSE 4 | 5 | #include 6 | #include 7 | 8 | namespace rai { 9 | namespace realsense { 10 | 11 | inline float get_depth_scale(rs2::device dev) { 12 | // Go over the device's sensors 13 | for(rs2::sensor& sensor : dev.query_sensors()) { 14 | // Check if the sensor if a depth sensor 15 | if(rs2::depth_sensor dpt = sensor.as()) { 16 | return dpt.get_depth_scale(); 17 | } 18 | } 19 | throw std::runtime_error("Device does not have a depth sensor"); 20 | } 21 | 22 | } 23 | } 24 | 25 | 26 | #endif // REALSENSE_UTILS_H 27 | -------------------------------------------------------------------------------- /src/Robotiq/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | NAME = $(shell basename `pwd`) 4 | OUTPUT = lib$(NAME).so 5 | 6 | DEPEND = Core 7 | 8 | SRCS = $(shell find . -maxdepth 1 -name '*.cpp' ) 9 | OBJS = $(SRCS:%.cpp=%.o) 10 | 11 | include $(BASE)/_make/generic.mk -------------------------------------------------------------------------------- /src/Robotiq/RobotiqGripper.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #ifdef RAI_ROBOTIQ 6 | # include 7 | # include 8 | #endif 9 | 10 | struct RobotiqGripper : rai::GripperAbstraction { 11 | double maxWidth; 12 | 13 | RobotiqGripper(uint whichRobot=0); 14 | ~RobotiqGripper(); 15 | 16 | void goTo(double force, double width, double speed); 17 | 18 | void open(double width, double speed){ goTo(0., width, speed); } 19 | void close(double force, double width, double speed){ goTo(force, width, speed); } 20 | 21 | double pos(); 22 | 23 | bool isDone(); 24 | 25 | private: 26 | #ifdef RAI_ROBOTIQ 27 | std::shared_ptr serialPort; 28 | #endif 29 | 30 | void getStatus(); 31 | }; 32 | -------------------------------------------------------------------------------- /src/Sim/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | NAME = $(shell basename `pwd`) 4 | OUTPUT = lib$(NAME).so 5 | 6 | DEPEND = Core Kin Gui Geo KOMO Optim RosCom Algo 7 | 8 | ROS = 1 9 | 10 | SRCS = $(shell find . -maxdepth 1 -name '*.cpp' ) 11 | OBJS = $(SRCS:%.cpp=%.o) 12 | 13 | include $(BASE)/_make/generic.mk 14 | -------------------------------------------------------------------------------- /src/Sim/simulationIO.cpp: -------------------------------------------------------------------------------- 1 | #include "simulationIO.h" 2 | #include 3 | #include "simulationIO_self.h" 4 | 5 | SimulationIO_self::SimulationIO_self(bool pubSubToROS, const char *modelFile, double dt) 6 | : ROS("simulator"), 7 | SIM(modelFile, dt), 8 | dt(dt), 9 | ref("MotionReference"), 10 | command("command"), 11 | currentQ("currentQ"), 12 | objectPoses("objectPoses"), 13 | objectNames("objectNames"), 14 | timeToGo("timeToGo"){ 15 | 16 | if(pubSubToROS){ 17 | //setup ros communication 18 | sub_ref = ROS.subscribe(ref); 19 | sub_command = ROS.subscribe(command); 20 | 21 | pub_timeToGo = ROS.publish(timeToGo); 22 | pub_objectPoses = ROS.publish(objectPoses); 23 | pub_currentQ = ROS.publish(currentQ); 24 | } 25 | } 26 | 27 | SimulationIO::SimulationIO(bool pubSubToROS, const char *modelFile, double dt) 28 | : Thread("SimulationIO", dt){ 29 | self = new SimulationIO_self(pubSubToROS, modelFile, dt); 30 | } 31 | 32 | SimulationIO::~SimulationIO(){ 33 | threadClose(); 34 | delete self; 35 | } 36 | 37 | void SimulationIO::step(){ 38 | //check for inputs: 39 | if(self->ref.hasNewRevision()){ 40 | self->ref.readAccess(); 41 | StringA joints = conv_stdStringVec2StringA(self->ref->joints); //is ignored for now!!! 42 | arr t = conv_arr2arr(self->ref->t); 43 | arr x = conv_arr2arr(self->ref->x); 44 | bool append = self->ref->append; 45 | cout <<"MotionReference revision=" <ref.getRevision() <<' ' <ref->revision <ref.deAccess(); 47 | 48 | self->SIM.setUsedRobotJoints(joints); 49 | self->SIM.exec(x, t, append); 50 | } 51 | 52 | if(self->command.hasNewRevision()){ 53 | StringA cmd = conv_StringA2StringA( self->command.get() ); 54 | self->SIM.exec(cmd); 55 | } 56 | 57 | self->SIM.stepKin(); 58 | 59 | //publish: 60 | self->currentQ.set() = conv_arr2arr( self->SIM.getJointState() ); 61 | self->objectNames.set() = conv_StringA2StringA( self->SIM.getObjectNames() ); 62 | self->objectPoses.set() = conv_arr2arr( self->SIM.getObjectPoses() ); 63 | self->timeToGo.set() = conv_double2Float64( self->SIM.getTimeToGo() ); 64 | } 65 | 66 | void SimulationIO::loop(){ 67 | //loop 68 | Metronome tictac(self->dt); 69 | for(;;){ 70 | tictac.waitForTic(); 71 | step(); 72 | } 73 | } 74 | 75 | -------------------------------------------------------------------------------- /src/Sim/simulationIO.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "simulation.h" 4 | 5 | struct SimulationIO : Thread{ 6 | struct SimulationIO_self *self=0; 7 | 8 | SimulationIO(bool pubSubToROS, const char* modelFile, double dt=.01); 9 | ~SimulationIO(); 10 | 11 | void step(); 12 | void close(){} 13 | void open(){} 14 | 15 | void loop(); 16 | }; 17 | -------------------------------------------------------------------------------- /src/Sim/simulationIO_self.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "simulationIO.h" 4 | 5 | struct SimulationIO_self{ 6 | RosCom ROS; // communication with ROS 7 | Simulation SIM; 8 | double dt; 9 | 10 | //inputs (reactive variables / messages) 11 | Var ref; // communication variable with ROS 12 | Var command; 13 | 14 | //outputs 15 | Var currentQ; 16 | Var objectPoses; 17 | Var objectNames; 18 | Var timeToGo; 19 | 20 | std::shared_ptr> sub_ref; //subscriber 21 | std::shared_ptr> sub_command; //subscriber 22 | std::shared_ptr> pub_currentQ; 23 | std::shared_ptr> pub_objectPoses; 24 | std::shared_ptr> pub_objectNames; 25 | std::shared_ptr> pub_timeToGo; 26 | 27 | SimulationIO_self(bool pubSubToROS, const char *modelFile, double dt); 28 | }; 29 | -------------------------------------------------------------------------------- /src/Sim/simulator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | struct Simulator{ 10 | private: 11 | RosCom ROS; // communication with ROS 12 | rai::KinematicWorld K, K_disp, K_ref; // kinematic configurations (K_disp only to display) 13 | OpenGL gl; // display 14 | 15 | //inputs (reactive variables / messages) 16 | Var ref; // communication variable with ROS 17 | Var command; 18 | 19 | //outputs 20 | Var currentQ; 21 | Var timeToGo; 22 | // Var percepts_input; 23 | 24 | 25 | std::shared_ptr> sub_ref; //subscriber 26 | std::shared_ptr> sub_command; //subscriber 27 | std::shared_ptr> pub_currentQ; 28 | std::shared_ptr> pub_timeToGo; 29 | 30 | rai::Spline reference; // reference spline constructed from ref 31 | arr refPoints, refTimes; // the knot points and times of the spline 32 | double phase=0.; // current phase in the spline 33 | double dt; // time stepping interval 34 | uint stepCount=0; // number of simulation steps 35 | 36 | public: 37 | Simulator(const char* modelFile, double dt=.01); 38 | void step(); 39 | void loop(); 40 | }; 41 | -------------------------------------------------------------------------------- /src/Utils/timerMacros.h: -------------------------------------------------------------------------------- 1 | #ifndef RAI_TIMER_MACROS 2 | #define RAI_TIMER_MACROS 3 | 4 | #include 5 | 6 | #define RAI_NOW std::chrono::high_resolution_clock::now() 7 | #define RAI_TIME(start) std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start).count() 8 | #define RAI_TIME_POINT std::chrono::time_point 9 | 10 | #endif 11 | -------------------------------------------------------------------------------- /src/ViveController: -------------------------------------------------------------------------------- 1 | ../../rai-contrib/ViveController -------------------------------------------------------------------------------- /src/brokenCodePieces/drake.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace drake{ 6 | struct sMyDrake; 7 | 8 | struct MyDrake{ 9 | sMyDrake *s; 10 | 11 | MyDrake(int argc, char* argv[]); 12 | ~MyDrake(); 13 | 14 | int DoMain(); 15 | 16 | void addKukaPlant(); 17 | void addController(); 18 | void addLogger(); 19 | void addReferenceTrajectory(const arr& knots=NoArr); 20 | 21 | void build(); 22 | 23 | void simulate(); 24 | void simulate2(); 25 | 26 | arr getLog(); 27 | 28 | //refactored from the mono demo 29 | void mono_setupGeometry(); 30 | void addMonoPlant(); 31 | void addWsgController(); 32 | void addPlanInterpolator(); 33 | void addStateMachine(); 34 | void addRAIMachine(); 35 | 36 | }; 37 | } 38 | -------------------------------------------------------------------------------- /src/brokenCodePieces/sendingAService.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | 6 | //=========================================================================== 7 | 8 | int query(const char* cmd){ 9 | rai_srv::StringString com; 10 | com.request.str = cmd; 11 | if(ros::service::call("/RAP/service", com)){ 12 | cout < 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | int main(int argc, char** argv) { 8 | rai::initCmdLine(argc, argv); 9 | 10 | rai::realsense::MultiRealSenseThread RS({"102422075114", "102422071099"}, {}, {}, true, false); 11 | uint V = RS.getNumberOfCameras(); 12 | std::vector windows(V); 13 | 14 | RS.color.waitForNextRevision(); 15 | for(uint i = 0; i < V; i++) { 16 | auto colorGet = RS.color.get(); 17 | windows[i].resize(colorGet()[i].d1, colorGet()[i].d0); 18 | } 19 | 20 | 21 | CycleTimer tim; 22 | for(;;) { 23 | RS.color.waitForNextRevision(); 24 | 25 | tim.cycleStart(); 26 | int key=0; 27 | for(uint i = 0; i < V; i++) { 28 | auto colorGet = RS.color.get(); 29 | key = windows[i].watchImage(colorGet()[i], false, 1.); 30 | } 31 | tim.cycleDone(); 32 | 33 | if(key=='q') break; 34 | } 35 | 36 | cout <<"DISPLAY timer: " < 2 | #include 3 | 4 | const char *USAGE = 5 | "\nTest of low-level (without bot interface) OptiTrack interface" 6 | "\n"; 7 | 8 | int main(int argc,char **argv){ 9 | rai::initCmdLine(argc, argv); 10 | 11 | cout <("optitrack/host")); 14 | 15 | rai::Configuration C; 16 | rai::OptiTrack OT; 17 | 18 | for(;;){ 19 | OT.pull(C); 20 | if(C.view(false)=='q') break; 21 | } 22 | 23 | LOG(0) <<" === bye bye ===\n used parameters:\n" < 3 | 4 | int rawTest(const char* type, const char* host) { 5 | // Make a new client 6 | libmotioncapture::MotionCapture *mocap = libmotioncapture::MotionCapture::connect(type, host); 7 | 8 | std::cout << "supportsRigidBodyTracking: " << mocap->supportsRigidBodyTracking() << std::endl; 9 | std::cout << "supportsLatencyEstimate: " << mocap->supportsLatencyEstimate() << std::endl; 10 | std::cout << "supportsPointCloud: " << mocap->supportsPointCloud() << std::endl; 11 | std::cout << "supportsTimeStamp: " << mocap->supportsTimeStamp() << std::endl; 12 | 13 | for (size_t frameId = 0;; ++frameId) 14 | { 15 | // Get a frame 16 | mocap->waitForNextFrame(); 17 | 18 | std::cout << "frame " << frameId << std::endl; 19 | if (mocap->supportsTimeStamp()) { 20 | std::cout << " timestamp: " << mocap->timeStamp() << " us" << std::endl; 21 | } 22 | 23 | if (mocap->supportsPointCloud()) { 24 | std::cout << " pointcloud:" << std::endl; 25 | auto pointcloud = mocap->pointCloud(); 26 | for (size_t i = 0; i < pointcloud.rows(); ++i) { 27 | const auto& point = pointcloud.row(i); 28 | std::cout << " \"" << i << "\": [" << point(0) << "," << point(1) << "," << point(2) << "]" << std::endl; 29 | } 30 | } 31 | 32 | if (mocap->supportsRigidBodyTracking()) { 33 | auto rigidBodies = mocap->rigidBodies(); 34 | 35 | std::cout << " rigid bodies:" << std::endl; 36 | 37 | for (auto const& item: rigidBodies) { 38 | const auto& rigidBody = item.second; 39 | 40 | std::cout << " \"" << rigidBody.name() << "\":" << std::endl; 41 | std::cout << " occluded: " << rigidBody.occluded() << std::endl; 42 | 43 | if (rigidBody.occluded() == false) { 44 | const auto& position = rigidBody.position(); 45 | const auto& rotation = rigidBody.rotation(); 46 | std::cout << " position: [" << position(0) << ", " << position(1) << ", " << position(2) << "]" << std::endl; 47 | std::cout << " rotation: [" << rotation.w() << ", " << rotation.vec()(0) << ", " 48 | << rotation.vec()(1) << ", " << rotation.vec()(2) << "]" << std::endl; 49 | } 50 | } 51 | } 52 | } 53 | 54 | return 0; 55 | } 56 | -------------------------------------------------------------------------------- /test/02-optitrack/rai.cfg: -------------------------------------------------------------------------------- 1 | optitrack/host: "130.149.82.29" 2 | optitrack/filter: .5 3 | -------------------------------------------------------------------------------- /test/03-blobTracking/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | 4 | DEPEND = Core Gui RealSense Perception 5 | 6 | OPENCV4 = 1 7 | 8 | include $(BASE)/_make/generic.mk 9 | -------------------------------------------------------------------------------- /test/03-blobTracking/rai.cfg: -------------------------------------------------------------------------------- 1 | RealSense/lowResolution:true 2 | RealSense/longCable:false 3 | RealSense/alignToDepth:true 4 | 5 | RealSense/autoExposure:false 6 | RealSense/exposure: 120 7 | RealSense/white: 3000 8 | 9 | #hsvFilter: [40, 100, 50, 55, 255, 255] 10 | hsvFilter: [70, 50, 100, 90, 255, 255] 11 | -------------------------------------------------------------------------------- /test/04-trivialCtrl/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | 4 | DEPEND = Core Algo Gui Geo Kin Franka Control 5 | 6 | include $(BASE)/_make/generic.mk 7 | -------------------------------------------------------------------------------- /test/04-trivialCtrl/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | const char *USAGE = 6 | "\nTest of low-level (without bot interface) FrankaThreadNew with trivial reference interface" 7 | "\n"; 8 | 9 | 10 | // define your own reference feed 11 | struct ZeroReference : rai::ReferenceFeed { 12 | Var position_ref; ///< if set, defines a non-zero velocity reference 13 | Var velocity_ref; ///< if set, defines a non-zero velocity reference 14 | 15 | ZeroReference& setVelocityReference(const arr& _velocity_ref){ velocity_ref.set() = _velocity_ref; return *this; } 16 | ZeroReference& setPositionReference(const arr& _position_ref){ position_ref.set() = _position_ref; return *this; } 17 | 18 | /// callback called by a robot control loop 19 | virtual void getReference(arr& q_ref, arr& qDot_ref, arr& qDDot_ref, const arr& q_real, const arr& qDot_real, double ctrlTime){ 20 | { 21 | arr pos = position_ref.get()(); 22 | if(pos.N) q_ref = pos; 23 | else q_ref = q_real; //->no position gains at all 24 | } 25 | { 26 | arr vel = velocity_ref.get()(); 27 | if(vel.N==1 && vel.scalar()==0.) qDot_ref.resize(qDot_real.N).setZero(); //[0] -> zero vel reference -> damping 28 | else if(vel.N) qDot_ref = vel; 29 | else qDot_ref = qDot_real; //[] -> no damping at all! 30 | } 31 | qDDot_ref.resize(q_ref.N).setZero(); 32 | } 33 | }; 34 | 35 | 36 | void test() { 37 | //-- setup a configuration 38 | rai::Configuration C; 39 | C.addFile(rai::raiPath("../rai-robotModels/scenarios/pandaSingle.g")); 40 | C.view(true); 41 | 42 | //-- start the franka thread 43 | C.ensure_indexedJoints(); 44 | FrankaThread robot(0, franka_getJointIndices(C,'l')); 45 | 46 | //comment the next line to only get gravity compensation instead of 'zero reference following' (which includes damping) 47 | auto ref = make_shared(); 48 | robot.cmd.set()->ref = ref; 49 | //ref->setPositionReference(robot.state.get()->q); 50 | //ref->setVelocityReference({.0,.0,.2,0,0,0,0}); 51 | 52 | for(;;){ 53 | if(C.view(false,STRING("time: "<q); 55 | rai::wait(.1); 56 | } 57 | } 58 | 59 | 60 | int main(int argc,char **argv){ 61 | rai::initCmdLine(argc, argv); 62 | 63 | cout < 2 | #include 3 | #include 4 | 5 | //=========================================================================== 6 | 7 | int main(int argc, char * argv[]){ 8 | rai::initCmdLine(argc, argv); 9 | 10 | //-- setup a configuration 11 | rai::Configuration C; 12 | if(rai::getParameter("bot/useArm", "left")=="left"){ 13 | C.addFile(rai::raiPath("../rai-robotModels/scenarios/pandaSingle.g")); 14 | }else{ 15 | C.addFile(rai::raiPath("../rai-robotModels/scenarios/pandasTable-calibrated.g")); 16 | } 17 | 18 | arr path = getLoopPath(C); 19 | 20 | //-- start a robot thread 21 | BotOp bot(C, rai::getParameter("real", false)); 22 | 23 | bot.setControllerWriteData(1); 24 | 25 | bot.home(C); 26 | 27 | //prepare storing optitrack data ? 28 | ofstream fil("z.calib.dat"); 29 | rai::Frame *optiFrameR=0, *optiFrameL = 0; 30 | if(bot.optitrack){ 31 | rai::wait(1.); 32 | optiFrameR = C["ot_r_panda_gripper"]; 33 | optiFrameL = C["ot_l_panda_gripper"]; 34 | } 35 | 36 | //-- send path as spline 37 | Metronome tic(.01); 38 | 39 | for(double speed=1.;speed<=3.;speed+=.5){ 40 | bot.move(path, arr{10.}/speed); 41 | 42 | if(bot.optitrack){ 43 | while(bot.sync(C, -1.)){ 44 | tic.waitForTic(); 45 | if(bot.optitrack){ 46 | arr q = bot.get_q(); 47 | bot.optitrack->pull(C); 48 | fil <ensure_X() <<" poseR " <ensure_X() <get_X() < 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | const char *USAGE = 11 | "\nTest of low-level (without bot interface) of SplineCtrlReference" 12 | "\n"; 13 | 14 | //=========================================================================== 15 | 16 | void test_bot() { 17 | //-- setup a configuration 18 | rai::Configuration C; 19 | C.addFile(rai::raiPath("../rai-robotModels/scenarios/pandaSingle.g")); 20 | 21 | //-- start a robot thread 22 | BotOp bot(C, rai::getParameter("real", false)); 23 | 24 | bot.gripperClose(rai::_left); 25 | 26 | //-- create 2 simple reference configurations 27 | arr q0 = bot.get_qHome(); 28 | arr qT = q0; 29 | qT(1) -= .5; 30 | 31 | bot.move(~qT, {2.}); //in 2 sec strict 32 | bot.wait(C, false, true); 33 | bot.moveTo(q0, 1.); //using timing cost=1 34 | bot.wait(C, false, true); 35 | 36 | bot.hold(false, true); 37 | 38 | for(;;){ 39 | bot.sync(C, .1); 40 | if(bot.keypressed=='q') break; 41 | 42 | arr y = C.eval(FS_position, {"l_gripper"}, {{1,3},{1,0,0}}); 43 | bot.setCompliance(y.J(), 1.); 44 | cout < 5 | shape:ssBox, size:[.015 .015 .5 .005] } 6 | 7 | stickTip (stick) { Q:[0 0 -.245], shape:sphere, size:[.008], color[1 1 .6 .3] } 8 | 9 | puck (table){ Q:[.0 .4 .08] 10 | shape:ssCylinder size:[.06 .06 .005] color:[1 1 .6] } 11 | 12 | target (table){ Q:[-.6 .3 .08] 13 | shape:ssCylinder size:[.06 .06 .005] color:[.6 1 .6] } 14 | -------------------------------------------------------------------------------- /test/13-reactiveGrasp/rai.cfg: -------------------------------------------------------------------------------- 1 | #real 2 | #bot/useArm:none 3 | #bot/useOptitrack: true 4 | 5 | KOMO/verbose: 0 6 | opt/maxStep = 0.1 7 | opt/verbose: 0 8 | 9 | bot/useGripper:true 10 | bot/useArm:both 11 | 12 | botemu/noise_th: .0 #.9 13 | 14 | bot/raiseWindow: true 15 | 16 | Franka/Kp_freq: [12., 12., 12., 12., 10., 15., 10.] 17 | -------------------------------------------------------------------------------- /test/14-calibPoints/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | 4 | #OBJS = main.o ../../bin/bot/bot.o 5 | #DEPEND = Core Algo Gui Geo Kin Optim KOMO Franka Control BotOp 6 | OPENCV = 1 7 | #OPENCV4 = 1 8 | 9 | include $(BASE)/_make/generic.mk 10 | -------------------------------------------------------------------------------- /test/14-calibPoints/calib: -------------------------------------------------------------------------------- 1 | total ERROR = 0.00452842 2 | *** total Pinv: 3 | [0.00315592, 0.000106899, -1.02654, -0.00219142, 4 | 3.80978e-05, -0.0031546, 0.550581, -0.00549389, 5 | -1.35119e-06, 2.42059e-05, -0.999362, -0.00735351] 6 | *** camera intrinsics K: 7 | [316.841, -6.90775, 320.502, 8 | 0, 316.882, 181.638, 9 | 0, 0, 1.00457] 10 | *** camera Fxypxy: 11 | [316.841, 316.882, 320.502, 181.638] 12 | *** camera world pose: [-0.00219142, -0.00549389, -0.00735351, 0.00384112, 0.999974, 0.00603637, -0.000190875] 13 | *** camera world pos: [-0.00219142, -0.00549389, -0.00735351] 14 | *** camera world rot: [0.00384112, 0.999974, 0.00603637, -0.000190875] 15 | -------------------------------------------------------------------------------- /test/14-calibPoints/rai.cfg: -------------------------------------------------------------------------------- 1 | real 2 | checks: 1 3 | 4 | #bot/useOptitrack: true 5 | optitrack/filter: .1 6 | botsim/engine: kinematic 7 | botsim/verbose: 0 8 | 9 | #RealSense/lowResolution: false 10 | #RealSense/autoExposure: true 11 | #RealSense/exposure: 100 12 | #RealSense/white: 2500 13 | 14 | dots: [0.180749, 0.101301, -0.295883, 0.0993051, -0.460925, -0.151945, -0.451812, -0.426068] 15 | dots: [0.180436, 0.101313, -0.294048, 0.10003, -0.459017, -0.15235, -0.452994, -0.425162] 16 | dots: [ 17 | 0.176193, 0.104552, 18 | -0.300173, 0.108897, 19 | -0.45, -0.1, 20 | -0.45, -0.5, 21 | ] 22 | views: 20 23 | 24 | #real day 25 | hsvFilter: [95, 200, 80, 26 | 110, 255, 200] 27 | 28 | #real in evening 29 | hsvFilter: [77, 105, 66, 30 | 135, 255, 235] 31 | 32 | 33 | #QcameraWrist: [0.0612469, -0.0228876, 0.156111, 0.36824, -0.0141277, 0.026341, -0.910506] 34 | QcameraWrist: [0.0566288, -0.0138618, 0.158583, 0.371288, -0.0124238, 0.0272688, -0.928034] 35 | QcameraWrist: [0.0569686, -0.0139042, 0.15845, 0.372332, -0.0123119, 0.0269895, -0.927625] 36 | [0.058572, -0.0142142, 0.160157, 0.927853, 0.0270032, 0.0137342, 0.371713] 37 | 38 | #real 39 | Pinv: [] 40 | 41 | Pinv:[-0.00231553, 0.00218067, 0.387915, 0.058572, 42 | -0.00221068, -0.00231521, 1.08355, -0.0142142, 43 | 1.7325e-05, -0.000192083, 1.02193, 0.160157] 44 | 45 | Pinv: [0.00315592, 0.000106899, -1.02654, -0.00219142, 46 | 3.80978e-05, -0.0031546, 0.550581, -0.00549389, 47 | -1.35119e-06, 2.42059e-05, -0.999362, -0.00735351] 48 | 49 | #sim 50 | Pinv: [0.00309787, -7.45426e-07, -0.989774, 7.7053e-06, 51 | -1.12853e-06, -0.00310151, 0.558699, -0.00046116, 52 | 1.11988e-06, -5.87783e-07, -1.00163, -0.000186689] 53 | 54 | opt/verbose: 0 55 | 56 | points:[ 57 | .15, .0, .1, 0, 0, 0 58 | .15, .6, .1, 0, 0, 0 59 | 1., .5, .1, 0, 0, 0 60 | .7, .4, .2, 0, 1, 0 61 | .7, -.5, .8, 0, 0, 1 62 | .7, .3, .7, -1, 0, 1 63 | .2, .5, .8, 1, 0, 1 64 | .3, .0, .8, 0, 0, 1 65 | ] 66 | 67 | demoPoints:[ 68 | .0, .0, .6 69 | .2, .2, .6 70 | .2, -.2, .6 71 | -.2, .0, .6 72 | ] 73 | -------------------------------------------------------------------------------- /test/14-calibPoints/station.g: -------------------------------------------------------------------------------- 1 | Include: <../../rai-robotModels/scenarios/pandaSingle.g> 2 | 3 | dot0 (table) { X:[0.176193, 0.104552, 0.6525], shape: cylinder, size: [.001 .02], color: [.2 .3 1] } 4 | dot1 (table) { X:[-0.300173, 0.108897, 0.6525], shape: cylinder, size: [.001 .02], color: [.2 .3 1] } 5 | #dot2 (table) { X:[-.25, 0.11, 0.65], shape: cylinder, size: [.001 .02], color: [.2 .3 1] } 6 | #dot3 (table) { X:[-.7, 0.11, 0.65], shape: cylinder, size: [.001 .02], color: [.2 .3 1] } 7 | -------------------------------------------------------------------------------- /test/20-simulation/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../../rai 2 | 3 | include $(BASE)/_make/generic.mk 4 | -------------------------------------------------------------------------------- /test/20-simulation/model.g: -------------------------------------------------------------------------------- 1 | Include: '../../rai-robotModels/scenarios/liftRing.g' 2 | 3 | Edit table {friction: 1.} 4 | Edit gripper { contact:-2 } 5 | Edit box { Q:, mass:.1, friction:1. } 6 | Edit stick { Q: } 7 | -------------------------------------------------------------------------------- /test/20-simulation/rai.cfg: -------------------------------------------------------------------------------- 1 | bullet/defaultFriction: 1. # default [d] 2 | bullet/defaultRestitution: 0.1 # default [d] 3 | bullet/contactStiffness: 10000 # default [d] 4 | bullet/contactDamping: 10.1 # default [d] 5 | 6 | physx/motorKp: 10000. 7 | physx/motorKd: 1000. 8 | physx/defaultRestitution: 10. 9 | #physx/defaultFriction: 10. 10 | #physx/angularDamping: 10. 11 | #physx/angularDamping: 10. 12 | -------------------------------------------------------------------------------- /test/21-grasp/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | 4 | OPENCV4 = 1 5 | 6 | CMAKE = 1 7 | 8 | include $(BASE)/_make/generic.mk 9 | 10 | -------------------------------------------------------------------------------- /test/21-grasp/rai.cfg: -------------------------------------------------------------------------------- 1 | #physx/motorKp: 1000. 2 | #physx/motorKd: 100. 3 | physx/gripperKp: 10000. 4 | physx/gripperKd: 100. 5 | #physx/defaultRestitution: 1. 6 | #physx/defaultFriction: 1.1 7 | #physx/angularDamping: 1. 8 | 9 | #botsim/verbose: 10 10 | #botsim/hyperSpeed: .5 11 | 12 | #botsim/engine: kinematic 13 | #physx/multiBody: false 14 | botsim/verbose: 0 #5 15 | #physx/verbose: 0 16 | -------------------------------------------------------------------------------- /test/py/Makefile: -------------------------------------------------------------------------------- 1 | 2 | ipynb_paths = $(shell find . -type f -name '*.ipynb' -not -name '*checkpoint*' -not -name '*nbconvert*' -printf "%f ") 3 | 4 | #run: 5 | # @for p in $(ipynb_paths); do jupyter-nbconvert --to notebook --execute $$p; done 6 | 7 | run: $(ipynb_paths:%=run/%) 8 | clean: $(ipynb_paths:%=clean/%) 9 | 10 | convert: $(ipynb_paths:%=convert/%) 11 | 12 | clean/%.ipynb: %.ipynb 13 | +@-jupyter-nbconvert --clear-output --inplace $< 14 | 15 | convert/%.ipynb: %.ipynb 16 | +@-jupyter-nbconvert --to python $< 17 | 18 | run/%.ipynb: %.ipynb 19 | +@-echo "=========== run $< =======" 20 | +@-jupyter-nbconvert --to notebook --execute $< 21 | rm -f *nbconvert* 22 | -------------------------------------------------------------------------------- /test/py/test.py: -------------------------------------------------------------------------------- 1 | import sys, os 2 | sys.path.append(os.path.expanduser('~/git/botop/build')) 3 | import libry as ry 4 | import numpy as np 5 | import time 6 | 7 | ry.params_add({ 'physx/motorKp': 10000., 'physx/motorKd': 1000., "bot/useArm": "both"}) 8 | ry.params_print() 9 | 10 | C = ry.Config() 11 | C.addFile(ry.raiPath('../rai-robotModels/scenarios/pandasTable.g')) 12 | C.view(False, 'this is your workspace data structure C -- NOT THE SIMULTATION') 13 | 14 | bot = ry.BotOp(C, True) 15 | 16 | bot.home(C) 17 | 18 | del bot 19 | -------------------------------------------------------------------------------- /test/retired/02-upDown/Makefile: -------------------------------------------------------------------------------- 1 | BASE = ../../rai 2 | BASE2 = ../.. 3 | 4 | DEPEND = Core Gui Geo Franka Kin Control 5 | 6 | include $(BASE)/_make/generic.mk 7 | -------------------------------------------------------------------------------- /test/retired/02-upDown/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | int main(int argc, char** argv) { 9 | Var sim_config; 10 | Var ctrl_config; 11 | Var ctrl_ref; 12 | Var ctrl_state; 13 | Var ctrl_tasks; 14 | 15 | { 16 | ctrl_config.set()->init("../../model/pandaStation.g"); 17 | sim_config.set() = ctrl_config.get(); 18 | arr q0 = ctrl_config.get()->getJointState(); 19 | { 20 | auto set = ctrl_state.set(); 21 | ctrl_config.set()->getJointState(set->q, set->qdot); 22 | } 23 | 24 | rai::Array> processes; 25 | 26 | if(rai::getParameter("sim", false)){ 27 | processes.append( make_shared(sim_config, ctrl_ref, ctrl_state) ); 28 | }else{ 29 | processes.append( make_shared(ctrl_ref, ctrl_state, 0, franka_getJointIndices(ctrl_config.get(), 'R')) ); 30 | processes.append( make_shared(ctrl_ref, ctrl_state, 1, franka_getJointIndices(ctrl_config.get(), 'L')) ); 31 | } 32 | 33 | TaskControlThread C(ctrl_config, ctrl_ref, ctrl_state, ctrl_tasks); 34 | ctrl_tasks.waitForRevisionGreaterThan(20); 35 | 36 | { 37 | auto t = addCtrlTask(ctrl_tasks, ctrl_config, "upDown", FS_position, {"endeffL"}, 2.); 38 | arr y0 = t->y; 39 | t->setTarget( y0 + ARR(.0, .0, .2) ); 40 | 41 | auto t2 = addCtrlTask(ctrl_tasks, ctrl_config, "upDown", FS_position, {"endeffR"}, 2.); 42 | arr y2 = t2->y; 43 | t2->setTarget( y2 + ARR(.0, .0, .2) ); 44 | 45 | wait(+t +t2); 46 | 47 | // t->setTarget(y0); 48 | // t2->setTarget(y2); 49 | // wait(+t +t2); 50 | } 51 | 52 | { 53 | auto t = addCtrlTask(C.ctrl_tasks, ctrl_config, "homing", FS_qItself, {}, 2.); 54 | t->setTarget( q0 ); 55 | wait(+ t); 56 | } 57 | } 58 | 59 | cout <<"BYE BYE" <