From 8939ca5b357d5fdd609b94b7bc4a9c4e06f2913a Mon Sep 17 00:00:00 2001 From: Marc Toussaint Date: Tue, 17 Oct 2023 16:30:51 +0200 Subject: [PATCH 1/7] v23 cleaned --- _build_utils/CMakeLists-docker.txt | 14 +- _build_utils/CMakeLists-ubuntu.txt | 2 +- _build_utils/build-wheels.sh | 7 +- examples/skeleton-solving-example.py | 85 -- notebooks/.gitignore | 4 - notebooks/1a-configurations.ipynb | 986 ------------------ notebooks/1b-botop.ipynb | 483 --------- notebooks/1c-komo.ipynb | 726 ------------- notebooks/1d-real.ipynb | 387 ------- notebooks/Makefile | 21 - notebooks/README.md | 13 - notebooks/botop3-vision.ipynb | 267 ----- notebooks/botop4-examples.ipynb | 629 ----------- notebooks/cameraview.ipynb | 156 --- notebooks/configurationEditing.ipynb | 363 ------- notebooks/features.ipynb | 657 ------------ notebooks/komo-reporting.ipynb | 510 --------- notebooks/komo3-switches-skeletons.ipynb | 314 ------ notebooks/mini.g | 3 - notebooks/opt1-basics.ipynb | 139 --- notebooks/rai.cfg | 4 - notebooks/retired/1-basics.ipynb | 562 ---------- notebooks/retired/2-features.ipynb | 287 ----- notebooks/retired/3-IK-optimization.ipynb | 378 ------- notebooks/retired/4-LGP.ipynb | 241 ----- notebooks/retired/4-path-optimization.ipynb | 149 --- notebooks/retired/5-cgo-optimization.ipynb | 152 --- notebooks/retired/6-KOMO-physics.ipynb | 190 ---- notebooks/retired/6-KOMO-skeleton.ipynb | 253 ----- notebooks/retired/9-robotModels.ipynb | 142 --- notebooks/retired/broken-6-KOMO-physics.ipynb | 190 ---- .../retired/broken-6-KOMO-skeleton.ipynb | 224 ---- notebooks/retired/lgp1-pickAndPlace.ipynb | 316 ------ notebooks/retired/sim1-physics.ipynb | 130 --- notebooks/script5-PathFind.ipynb | 227 ---- notebooks/sim1-physics.ipynb | 141 --- rai | 2 +- rai-robotModels | 2 +- robotic/render.py | 122 +++ robotic/ry-view | 24 + robotic/version.py | 2 +- setup.py | 4 +- 42 files changed, 166 insertions(+), 9342 deletions(-) delete mode 100644 examples/skeleton-solving-example.py delete mode 100644 notebooks/.gitignore delete mode 100644 notebooks/1a-configurations.ipynb delete mode 100644 notebooks/1b-botop.ipynb delete mode 100644 notebooks/1c-komo.ipynb delete mode 100644 notebooks/1d-real.ipynb delete mode 100644 notebooks/Makefile delete mode 100644 notebooks/README.md delete mode 100644 notebooks/botop3-vision.ipynb delete mode 100644 notebooks/botop4-examples.ipynb delete mode 100644 notebooks/cameraview.ipynb delete mode 100644 notebooks/configurationEditing.ipynb delete mode 100644 notebooks/features.ipynb delete mode 100644 notebooks/komo-reporting.ipynb delete mode 100644 notebooks/komo3-switches-skeletons.ipynb delete mode 100644 notebooks/mini.g delete mode 100644 notebooks/opt1-basics.ipynb delete mode 100644 notebooks/rai.cfg delete mode 100644 notebooks/retired/1-basics.ipynb delete mode 100644 notebooks/retired/2-features.ipynb delete mode 100644 notebooks/retired/3-IK-optimization.ipynb delete mode 100644 notebooks/retired/4-LGP.ipynb delete mode 100644 notebooks/retired/4-path-optimization.ipynb delete mode 100644 notebooks/retired/5-cgo-optimization.ipynb delete mode 100644 notebooks/retired/6-KOMO-physics.ipynb delete mode 100644 notebooks/retired/6-KOMO-skeleton.ipynb delete mode 100644 notebooks/retired/9-robotModels.ipynb delete mode 100644 notebooks/retired/broken-6-KOMO-physics.ipynb delete mode 100644 notebooks/retired/broken-6-KOMO-skeleton.ipynb delete mode 100644 notebooks/retired/lgp1-pickAndPlace.ipynb delete mode 100644 notebooks/retired/sim1-physics.ipynb delete mode 100644 notebooks/script5-PathFind.ipynb delete mode 100644 notebooks/sim1-physics.ipynb create mode 100644 robotic/render.py create mode 100755 robotic/ry-view diff --git a/_build_utils/CMakeLists-docker.txt b/_build_utils/CMakeLists-docker.txt index 17f2c07fb..c3ef3dd0b 100644 --- a/_build_utils/CMakeLists-docker.txt +++ b/_build_utils/CMakeLists-docker.txt @@ -106,7 +106,7 @@ if(USE_BULLET) find_package(Bullet REQUIRED) add_definitions(-DRAI_BULLET) include_directories(${BULLET_INCLUDE_DIRS}) -# target_link_libraries(rai ${BULLET_LIBRARIES}) + target_link_libraries(rai ${BULLET_LIBRARIES}) message(STATUS "[rai] using bullet libs:" ${BULLET_LIBRARIES}) endif() @@ -168,8 +168,16 @@ endif() ################################################################################ -#add_executable(kinEdit rai/bin/src_kinEdit/main.cpp) -#target_link_libraries(kinEdit rai) +add_executable(raiView rai/bin/src_kinEdit/main.cpp) +target_link_libraries(raiView PRIVATE + rai + dl libjsoncpp.a lapack pthread rt X11 libatlas.a + libassimp.a libglfw3.a libGLEW.a glut GLU GL libpng16.a libzlibstatic.a libqhull.a libANN.a libccd.a libfcl.a + #${BULLET_LIBRARIES} + libBullet2FileLoader.a libBullet3Common.a libBullet3Geometry.a libBulletCollision.a libBulletInverseDynamics.a + libBullet3Collision.a libBullet3Dynamics.a libBullet3OpenCL_clew.a libBulletDynamics.a libBulletSoftBody.a + libLinearMath.a + ) #add_executable(testSim rai/test/Kin/simulation/main.cpp) #target_link_libraries(testSim rai) diff --git a/_build_utils/CMakeLists-ubuntu.txt b/_build_utils/CMakeLists-ubuntu.txt index f1b5e99d1..3206c112c 100644 --- a/_build_utils/CMakeLists-ubuntu.txt +++ b/_build_utils/CMakeLists-ubuntu.txt @@ -181,7 +181,7 @@ message(STATUS "[rai] compiler flags: " "${_defs}") ################################################################################ -add_custom_target(docstrings ALL +add_custom_target(docstrings #ALL DEPENDS ry COMMAND pybind11-stubgen ry COMMAND mv stubs/ry*/__init__.pyi ry.pyi) diff --git a/_build_utils/build-wheels.sh b/_build_utils/build-wheels.sh index d9d90d096..c889ff18f 100755 --- a/_build_utils/build-wheels.sh +++ b/_build_utils/build-wheels.sh @@ -6,7 +6,6 @@ mkdir -p build ### delete old rm -Rf robotic/*ry.* robotic/*~ robotic/__pycache__ dist/ build/bdist* build/lib robotic.egg-info -unalias cp ### copy robotModels files cd robotic @@ -31,13 +30,13 @@ for ver in 8 9 10 6 7; do cmake -DPYBIND11_PYTHON_VERSION=3.$ver .. && make ry strip --strip-unneeded ry.*3$ver*.so echo -e "\n\n======== documenting (python version " $ver ") ========" - /opt/_internal/cpython-3.$ver.*/bin/pybind11-stubgen --ignore-invalid=all ry + /opt/_internal/cpython-3.$ver.*/bin/pybind11-stubgen ry cd .. echo -e "\n\n======== build wheel (python version " $ver ") ========" cp -f build/ry.*3$ver*.so robotic/ry.so - cp -f build/stubs/ry-stubs/__init__.pyi robotic/ry.pyi + cp -f build/stubs/ry*/__init__.pyi robotic/ry.pyi python3.$ver setup.py bdist_wheel - #break + break done echo -e "\n\n======== renaming wheels ========" diff --git a/examples/skeleton-solving-example.py b/examples/skeleton-solving-example.py deleted file mode 100644 index 791cf2c4e..000000000 --- a/examples/skeleton-solving-example.py +++ /dev/null @@ -1,85 +0,0 @@ -import sys -sys.path.append('../build') -import time -import numpy as np -from robotic import ry -#import libry as ry - -## create a configuration -C = ry.Config() -C.addFile(ry.raiPath('pr2/pr2.g')) -C.addFile(ry.raiPath('objects/tables.g')) -C.addFrame('obj0', 'table1', 'type:ssBox size:[.1 .1 .2 .02] color:[1. 0. 0.], contact, logical={ object }, joint:rigid, Q:' ) -C.addFrame('obj1', 'table1', 'type:ssBox size:[.1 .1 .2 .02] color:[1. 0. 0.], contact, logical={ object }, joint:rigid, Q:' ) -C.addFrame('obj2', 'table1', 'type:ssBox size:[.1 .1 .2 .02] color:[1. 0. 0.], contact, logical={ object }, joint:rigid, Q:' ) -C.addFrame('obj3', 'table1', 'type:ssBox size:[.1 .1 .2 .02] color:[1. 0. 0.], contact, logical={ object }, joint:rigid, Q:' ) -C.addFrame('tray', 'table2', 'type:ssBox size:[.15 .15 .04 .02] color:[0. 1. 0.], logical={ table }, Q:' ); -C.addFrame('', 'tray', 'type:ssBox size:[.27 .27 .04 .02] color:[0. 1. 0.]' ) -#C.view(False, 'initial model') - -## create a skeleton -S = ry.Skeleton() -S.addEntry([1.], ry.SY.touch, ['pr2R', 'obj0']) -S.addEntry([1., 3.], ry.SY.stable, ['pr2R', 'obj0']) -S.addEntry([2.,2.], ry.SY.touch, ['pr2L', 'obj3']) -S.addEntry([2.,4.], ry.SY.stable, ['pr2L', 'obj3']) -S.addEntry([3.], ry.SY.above, ['obj0', 'tray']) -S.addEntry([3.,4.], ry.SY.stableOn, ['tray', 'obj0']) - -## solve for waypoints: create a komo instance, create nlp instance, then call generic solver -komo = S.getKomo_waypoints(C, 1e-1, 1e-2) -nlp = komo.nlp() -sol = ry.NLP_Solver() -sol.setProblem(nlp) -sol.setOptions( stopTolerance=1e-2 ) -ret = sol.solve() -waypoints = komo.getPath_qAll() -# report on result, view, and play -print(ret) -#print(nlpW.report(2)) -komo.view(True, 'waypoints solution') -komo.view_play(True, .2) -# store result - -## solve for paths using RRT: for each phase create start-end problems, run RRT -m = len(waypoints) -rrt_dofs = [] -rrt_paths = [] -for t in range(0,int(m)): - # grab config and waypoints - [Ctmp, q0, q1] = S.getTwoWaypointProblem(t, komo) - Ctmp.setJointState(q0); - Ctmp.view(True, 'waypoint configuration phase ' + str(t) + ' START') - Ctmp.setJointState(q1); - Ctmp.view(True, 'waypoint configuration phase ' + str(t) + ' STOP') - - # call path finder - sol = ry.PathFinder() - sol.setProblem(Ctmp, q0, q1) - ret = sol.solve() - rrt_paths.append(ret.x) - rrt_dofs.append(Ctmp.getDofIDs()) - - #display the rrt path - for i in range(0,ret.x.shape[0]): - Ctmp.setJointState(ret.x[i]) - Ctmp.view(False, 'rrt path ' + str(i)) - time.sleep(.02) - -## solve for full path: create a komo instance, initialize with waypoints & rrt paths, solve -komo = S.getKomo_path(C, 20, .2, -1, 1e-2) -komo.initWithWaypoints(waypoints) -komo.view(True, 'init with waypoints only') -for t in range(0,int(m)): - komo.initPhaseWithDofsPath(t, rrt_dofs[t], rrt_paths[t], True) - komo.view(True, 'init with RRT phase ' + str(t)) -nlp = komo.nlp() -sol = ry.NLP_Solver() -sol.setProblem(nlp) -sol.setOptions( stopTolerance=1e-2 ) -ret = sol.solve() -# report on result, view, and play -print(ret) -#print(nlp.report(2)) -komo.view(True, 'path solution') -komo.view_play(True, .2) diff --git a/notebooks/.gitignore b/notebooks/.gitignore deleted file mode 100644 index 86c31b53c..000000000 --- a/notebooks/.gitignore +++ /dev/null @@ -1,4 +0,0 @@ -*.html -.ipynb_checkpoints -*.nbconvert.ipynb -tmp.g diff --git a/notebooks/1a-configurations.ipynb b/notebooks/1a-configurations.ipynb deleted file mode 100644 index 82e10e0ad..000000000 --- a/notebooks/1a-configurations.ipynb +++ /dev/null @@ -1,986 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Configurations\n", - "\n", - "A configuration is essentially a set of (coordinate) frames, where each frame can represent a shape, joint, inertia, etc. This tutorial introduces to basics of creating & loading configurations, the joint and frame state, computing features, and handling the view window." - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "compile time: Sep 30 2023 22:39:07\n" - ] - } - ], - "source": [ - "from robotic import ry\n", - "import numpy as np\n", - "import time\n", - "print(ry.compiled())" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Adding frames to a configuration\n", - "\n", - "The starting point is to create a `Configuration`:" - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 2, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C = ry.Config()\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "This shows an empty configuration. Tip: Make the view window appear \"Always On Top\" (right click on the window bar)\n", - "\n", - "A configuration is essentially a tree (or forrest) of frames. You usually add models from files, but let's do it manually here." - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "frame name: first pos: [0. 0. 0.5] quat: [0.95782629 0.28734789 0. 0. ]\n" - ] - }, - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 3, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C.clear()\n", - "f = C.addFrame(name='first')\n", - "f.setShape(type=ry.ST.marker, size=[.4])\n", - "f.setPosition([0.,0.,.5])\n", - "f.setQuaternion([1., .3, .0, .0]) #is normalized internally\n", - "print('frame name:', f.name, 'pos:', f.getPosition(), 'quat:', f.getQuaternion())\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Let's add a second frame, but with first as parent and with a hinge joint!" - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "frame name: second pos: [0. 0. 0.5] quat: [-0.95782629 -0.28734789 -0. -0. ]\n" - ] - }, - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 4, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "f = C.addFrame(name='second', parent='first')\n", - "f.setJoint(ry.JT.hingeX)\n", - "f.setShape(type=ry.ST.marker, size=[.4])\n", - "f.setColor([1,0,0])\n", - "print('frame name:', f.name, 'pos:', f.getPosition(), 'quat:', f.getQuaternion())\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Since we now have a configuration with a joint, we can articulate it:" - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "joint state: [0.1]\n" - ] - }, - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 5, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "q = C.getJointState()\n", - "q[0] = q[0] + .1\n", - "C.setJointState(q)\n", - "print('joint state:', q)\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Other examples to add:" - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "metadata": {}, - "outputs": [], - "source": [ - "C.addFrame('ball', 'second') .setShape(ry.ST.sphere, [.1]) .setColor([1.,.5,.0]) .setRelativePosition([-.3,.0,.2])\n", - "C.addFrame('box', 'second') .setShape(ry.ST.ssBox, [.3,.2,.1,.02]) .setColor([.5,1.,.0]) .setRelativePosition([.0,.0,.2])\n", - "C.addFrame('capsule', 'second') .setShape(ry.ST.capsule, [.3, .05]) .setColor([.0,1.,.5]) .setRelativePosition([.3,.0,.2])\n", - "for t in range(100):\n", - " C.setJointState([np.cos(.1*t)])\n", - " C.view()\n", - " time.sleep(.1)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Loading existing configurations" - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 7, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C.clear()\n", - "C.addFile(ry.raiPath('panda/panda.g'))\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Let's add a second panda, but prefix all frame names, and move it to the side" - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 8, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C.addFile(ry.raiPath('panda/panda.g'), 'r_')\n", - "base_r = C.getFrame('r_panda_base')\n", - "base_r.setPosition([.0, .5, .0])\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "We can get the joint state of the full configuration:" - ] - }, - { - "cell_type": "code", - "execution_count": 9, - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "[ 0. -1. 0. -2. 0. 2. 0. 0.05 0. -1. 0. -2.\n", - " 0. 2. 0. 0.05]\n", - "joints: ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7', 'panda_finger_joint1panda_finger_joint2', 'r_panda_joint1', 'r_panda_joint2', 'r_panda_joint3', 'r_panda_joint4', 'r_panda_joint5', 'r_panda_joint6', 'r_panda_joint7', 'r_panda_finger_joint1r_panda_finger_joint2']\n", - "frames: ['panda_base', 'panda_link0', 'panda_link0_0', 'panda_joint1_origin', 'panda_joint1', 'panda_link1', 'panda_link1_0', 'panda_joint2_origin', 'panda_joint2', 'panda_link2', 'panda_link2_0', 'panda_joint3_origin', 'panda_joint3', 'panda_link3', 'panda_link3_0', 'panda_joint4_origin', 'panda_joint4', 'panda_link4', 'panda_link4_0', 'panda_joint5_origin', 'panda_joint5', 'panda_link5', 'panda_link5_0', 'panda_joint6_origin', 'panda_joint6', 'panda_link6', 'panda_link6_0', 'panda_joint7_origin', 'panda_joint7', 'panda_link7', 'panda_link7_0', 'panda_joint8_origin', 'panda_joint8', 'panda_link8', 'panda_hand_joint_origin', 'panda_hand_joint', 'panda_hand', 'panda_hand_0', 'panda_finger_joint1_origin', 'panda_finger_joint2_origin', 'panda_finger_joint1', 'panda_finger_joint2', 'panda_leftfinger', 'panda_rightfinger', 'panda_leftfinger_0', 'panda_rightfinger_0', 'panda_coll0', 'panda_coll0b', 'panda_coll1', 'panda_coll3', 'panda_coll5', 'panda_coll2', 'panda_coll4', 'panda_coll6', 'panda_coll7', 'gripper', 'palm', 'finger1', 'finger2', 'r_panda_base', 'r_panda_link0', 'r_panda_link0_0', 'r_panda_joint1_origin', 'r_panda_joint1', 'r_panda_link1', 'r_panda_link1_0', 'r_panda_joint2_origin', 'r_panda_joint2', 'r_panda_link2', 'r_panda_link2_0', 'r_panda_joint3_origin', 'r_panda_joint3', 'r_panda_link3', 'r_panda_link3_0', 'r_panda_joint4_origin', 'r_panda_joint4', 'r_panda_link4', 'r_panda_link4_0', 'r_panda_joint5_origin', 'r_panda_joint5', 'r_panda_link5', 'r_panda_link5_0', 'r_panda_joint6_origin', 'r_panda_joint6', 'r_panda_link6', 'r_panda_link6_0', 'r_panda_joint7_origin', 'r_panda_joint7', 'r_panda_link7', 'r_panda_link7_0', 'r_panda_joint8_origin', 'r_panda_joint8', 'r_panda_link8', 'r_panda_hand_joint_origin', 'r_panda_hand_joint', 'r_panda_hand', 'r_panda_hand_0', 'r_panda_finger_joint1_origin', 'r_panda_finger_joint2_origin', 'r_panda_finger_joint1', 'r_panda_finger_joint2', 'r_panda_leftfinger', 'r_panda_rightfinger', 'r_panda_leftfinger_0', 'r_panda_rightfinger_0', 'r_panda_coll0', 'r_panda_coll0b', 'r_panda_coll1', 'r_panda_coll3', 'r_panda_coll5', 'r_panda_coll2', 'r_panda_coll4', 'r_panda_coll6', 'r_panda_coll7', 'r_gripper', 'r_palm', 'r_finger1', 'r_finger2']\n" - ] - } - ], - "source": [ - "print(C.getJointState())\n", - "print('joints:', C.getJointNames())\n", - "print('frames:', C.getFrameNames())" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Let's animate:" - ] - }, - { - "cell_type": "code", - "execution_count": 10, - "metadata": {}, - "outputs": [], - "source": [ - "q0 = C.getJointState()\n", - "for t in range(20):\n", - " q = q0 + .1*np.random.randn(q0.shape[0])\n", - " C.setJointState(q)\n", - " C.view()\n", - " time.sleep(.2)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Features: computing geometric properties\n", - "For every frame we can query its pose:" - ] - }, - { - "cell_type": "code", - "execution_count": 11, - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "gripper pos: [0.32141215 0.44345913 0.72576189]\n", - "gripper quat: [-0.8669439 -0.10911532 0.39042322 0.28995142]\n", - "gripper rot: [[ 0.52699576 0.41754092 -0.74022635]\n", - " [-0.58794555 0.80804404 0.03721381]\n", - " [ 0.61367378 0.41560126 0.6713271 ]]\n" - ] - } - ], - "source": [ - "f = C.getFrame('r_gripper')\n", - "print('gripper pos:', f.getPosition())\n", - "print('gripper quat:', f.getQuaternion())\n", - "print('gripper rot:', f.getRotationMatrix())" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The above provides basic forward kinematics: After `setJointState` you can query the pose of any configuration frame. However, there is a more general way to query *features*, i.e. properties of the configuration in a differentiable manner. You might not use this often; but it is important to understand as these differentiable features are the foundation of how optimization problems are formulated, which you'll need a lot.\n", - "\n", - "Here are some example features to evaluate:" - ] - }, - { - "cell_type": "code", - "execution_count": 12, - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "position of gripper: [ 0.32842615 -0.00310221 0.76643017] \n", - "Jacobian: [[ 3.10221333e-03 4.33076454e-01 -1.14787661e-02 -1.67755538e-01\n", - " -3.79268649e-03 8.75715712e-02 8.67361738e-19 0.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00]\n", - " [ 3.28426146e-01 1.75069738e-02 5.42979997e-01 -1.54572121e-02\n", - " 1.81774030e-01 -3.01246858e-03 2.77555756e-17 0.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00]\n", - " [ 0.00000000e+00 -3.28032821e-01 1.25841880e-02 5.18337387e-01\n", - " 3.99599798e-03 2.20150278e-01 1.73472348e-18 0.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00]]\n" - ] - } - ], - "source": [ - "[y,J] = C.eval(ry.FS.position, ['gripper'])\n", - "print('position of gripper:', y, '\\nJacobian:', J)" - ] - }, - { - "cell_type": "code", - "execution_count": 13, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "(array([-0.31946979]),\n", - " array([[ 2.11266970e-01, 2.46852305e-02, 5.50265616e-01,\n", - " -4.20418790e-02, 2.95705159e-02, -1.11343269e-02,\n", - " -2.43657527e-17, -0.00000000e+00, -1.96930432e-01,\n", - " -4.35913337e-02, -5.21550074e-01, 7.40963736e-02,\n", - " -1.50353913e-02, 5.41474581e-03, -5.27869680e-18,\n", - " -0.00000000e+00]]))" - ] - }, - "execution_count": 13, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "# negative(!) distance between two convex shapes (or origin of marker)\n", - "C.eval(ry.FS.negDistance, ['panda_coll7', 'r_panda_coll7'])" - ] - }, - { - "cell_type": "code", - "execution_count": 14, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "(array([ 0.51342582, -0.68935383, 0.51106284]),\n", - " array([[ 0.68935383, 0.51064577, 0.42483481, -0.52659502, 0.332467 ,\n", - " -0.5259756 , -0.45868217, 0. , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. , 0. ,\n", - " 0. ],\n", - " [ 0.51342582, 0.02064269, 0.72089851, -0.02266834, -0.20468464,\n", - " -0.02193998, -0.72379283, 0. , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. , 0. ,\n", - " 0. ],\n", - " [ 0. , -0.48516266, 0.54559434, 0.49845333, -0.61009578,\n", - " 0.4988135 , -0.51549452, 0. , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. , 0. ,\n", - " 0. ]]))" - ] - }, - "execution_count": 14, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "# the x-axis of the given frame in world coordinates\n", - "C.eval(ry.FS.vectorX, ['gripper'])" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Joint and Frame State\n", - "\n", - "A configuration is a tree of n frames. Every frame has a pose (position & quaternion), which is represented as a 7D vector (x,y,z, qw,qx,qy,qz). The frame state is the $n\\times 7$ matrix, where the i-th row is the pose of the i-th frame.\n", - "\n", - "A configuration also defines joints, which means that the relative transfromation from a parent to a child frame is parameterized by degrees-of-freedoms (DOFs). If the configuration has in total n DOFs, the joint state is a n-dimensional vector.\n", - "\n", - "Setting the joint state implies computing all relative transformations, and then forward chaining all transformations to compute all frame poses. So setting the joint state also sets the frame state.\n", - " \n", - "Setting the frame state allows you to set frame poses that are inconsistent/impossible w.r.t. the joints! Setting the frame state implies computing all relative transformations from the frame poses, and then assigning the joint state to the *projection* onto the actual DOFs" - ] - }, - { - "cell_type": "code", - "execution_count": 15, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 15, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C.setJointState(q0)\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The *frame state* is a $n\\times 7$ matrix, which contains for all of $n$ frames the 7D pose. A pose is stored as [p_x, p_y, p_z, q_w, q_x, q_y, q_z], with position p and quaternion q." - ] - }, - { - "cell_type": "code", - "execution_count": 16, - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "frame state: [[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00]\n", - " [ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00]\n", - " [ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00]\n", - " [ 0.00000000e+00 0.00000000e+00 3.33000000e-01 1.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00]\n", - " [ 0.00000000e+00 0.00000000e+00 3.33000000e-01 1.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00]\n", - " [ 0.00000000e+00 0.00000000e+00 3.33000000e-01 1.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00]\n", - " [ 0.00000000e+00 0.00000000e+00 3.33000000e-01 1.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00]\n", - " [ 0.00000000e+00 0.00000000e+00 3.33000000e-01 7.07106781e-01\n", - " -7.07106781e-01 0.00000000e+00 0.00000000e+00]\n", - " [ 0.00000000e+00 0.00000000e+00 3.33000000e-01 6.20544581e-01\n", - " -6.20544581e-01 -3.39005049e-01 -3.39005049e-01]\n", - " [ 0.00000000e+00 0.00000000e+00 3.33000000e-01 6.20544581e-01\n", - " -6.20544581e-01 -3.39005049e-01 -3.39005049e-01]\n", - " [ 0.00000000e+00 0.00000000e+00 3.33000000e-01 6.20544581e-01\n", - " -6.20544581e-01 -3.39005049e-01 -3.39005049e-01]\n", - " [-2.65904831e-01 -1.05249143e-16 5.03735529e-01 8.77582562e-01\n", - " 0.00000000e+00 -4.79425539e-01 0.00000000e+00]\n", - " [-2.65904831e-01 -1.05249143e-16 5.03735529e-01 8.77582562e-01\n", - " 0.00000000e+00 -4.79425539e-01 0.00000000e+00]\n", - " [-2.65904831e-01 -1.05249143e-16 5.03735529e-01 8.77582562e-01\n", - " 0.00000000e+00 -4.79425539e-01 0.00000000e+00]\n", - " [-2.65904831e-01 -1.05249143e-16 5.03735529e-01 8.77582562e-01\n", - " 0.00000000e+00 -4.79425539e-01 0.00000000e+00]\n", - " [-2.21329891e-01 -1.05249143e-16 5.73156885e-01 6.20544581e-01\n", - " 6.20544581e-01 -3.39005049e-01 3.39005049e-01]\n", - " [-2.21329891e-01 -1.05249143e-16 5.73156885e-01 6.20544581e-01\n", - " 6.20544581e-01 3.39005049e-01 -3.39005049e-01]\n", - " [-2.21329891e-01 -1.05249143e-16 5.73156885e-01 6.20544581e-01\n", - " 6.20544581e-01 3.39005049e-01 -3.39005049e-01]\n", - " [-2.21329891e-01 -1.05249143e-16 5.73156885e-01 6.20544581e-01\n", - " 6.20544581e-01 3.39005049e-01 -3.39005049e-01]\n", - " [ 5.72200270e-02 2.67785794e-16 8.50054327e-01 8.77582562e-01\n", - " 0.00000000e+00 4.79425539e-01 0.00000000e+00]\n", - " [ 5.72200270e-02 2.67785794e-16 8.50054327e-01 8.77582562e-01\n", - " 0.00000000e+00 4.79425539e-01 0.00000000e+00]\n", - " [ 5.72200270e-02 2.67785794e-16 8.50054327e-01 8.77582562e-01\n", - " 0.00000000e+00 4.79425539e-01 0.00000000e+00]\n", - " [ 5.72200270e-02 2.67785794e-16 8.50054327e-01 8.77582562e-01\n", - " 0.00000000e+00 4.79425539e-01 0.00000000e+00]\n", - " [ 5.72200270e-02 2.67785794e-16 8.50054327e-01 6.20544581e-01\n", - " 6.20544581e-01 3.39005049e-01 -3.39005049e-01]\n", - " [ 5.72200270e-02 2.67785794e-16 8.50054327e-01 6.20544581e-01\n", - " 6.20544581e-01 -3.39005049e-01 3.39005049e-01]\n", - " [ 5.72200270e-02 2.67785794e-16 8.50054327e-01 6.20544581e-01\n", - " 6.20544581e-01 -3.39005049e-01 3.39005049e-01]\n", - " [ 5.72200270e-02 2.67785794e-16 8.50054327e-01 6.20544581e-01\n", - " 6.20544581e-01 -3.39005049e-01 3.39005049e-01]\n", - " [ 1.04766630e-01 2.67785794e-16 9.24103773e-01 0.00000000e+00\n", - " 8.77582562e-01 0.00000000e+00 4.79425539e-01]\n", - " [ 1.04766630e-01 2.67785794e-16 9.24103773e-01 0.00000000e+00\n", - " 8.77582562e-01 0.00000000e+00 4.79425539e-01]\n", - " [ 1.04766630e-01 2.67785794e-16 9.24103773e-01 0.00000000e+00\n", - " 8.77582562e-01 0.00000000e+00 4.79425539e-01]\n", - " [ 1.04766630e-01 2.67785794e-16 9.24103773e-01 0.00000000e+00\n", - " 8.77582562e-01 0.00000000e+00 4.79425539e-01]\n", - " [ 1.94804025e-01 2.67785794e-16 8.66291427e-01 0.00000000e+00\n", - " 8.77582562e-01 0.00000000e+00 4.79425539e-01]\n", - " [ 1.94804025e-01 2.67785794e-16 8.66291427e-01 0.00000000e+00\n", - " 8.77582562e-01 0.00000000e+00 4.79425539e-01]\n", - " [ 1.94804025e-01 2.67785794e-16 8.66291427e-01 0.00000000e+00\n", - " 8.77582562e-01 0.00000000e+00 4.79425539e-01]\n", - " [ 1.94804025e-01 2.67785794e-16 8.66291427e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 1.94804025e-01 2.67785794e-16 8.66291427e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 1.94804025e-01 2.67785794e-16 8.66291427e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 1.94804025e-01 2.67785794e-16 8.66291427e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 2.43945931e-01 2.71027645e-16 8.34737772e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 2.43945931e-01 2.71027645e-16 8.34737772e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 2.63048480e-01 -3.53553800e-02 8.64488229e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 2.24843382e-01 3.53553800e-02 8.04987314e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 2.63048480e-01 -3.53553800e-02 8.64488229e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 2.24843382e-01 3.53553800e-02 8.04987314e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 2.63048480e-01 -3.53553800e-02 8.64488229e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 2.24843382e-01 3.53553800e-02 8.04987314e-01 -4.42931549e-01\n", - " 3.35835838e-01 -8.10780761e-01 1.83467955e-01]\n", - " [-4.00000000e-02 0.00000000e+00 3.00000000e-02 7.07106781e-01\n", - " 0.00000000e+00 7.07106781e-01 0.00000000e+00]\n", - " [-2.00000000e-01 -1.20000000e-01 0.00000000e+00 7.07106781e-01\n", - " 0.00000000e+00 7.07106781e-01 0.00000000e+00]\n", - " [ 0.00000000e+00 0.00000000e+00 1.83000000e-01 1.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00]\n", - " [-1.39684183e-01 -1.05249143e-16 4.22690183e-01 8.77582562e-01\n", - " 0.00000000e+00 -4.79425539e-01 0.00000000e+00]\n", - " [-1.11074170e-01 2.00000000e-02 7.41993865e-01 8.77582562e-01\n", - " 0.00000000e+00 4.79425539e-01 0.00000000e+00]\n", - " [ 0.00000000e+00 0.00000000e+00 3.33000000e-01 6.20544581e-01\n", - " -6.20544581e-01 -3.39005049e-01 -3.39005049e-01]\n", - " [-2.21329891e-01 -1.05249143e-16 5.73156885e-01 6.20544581e-01\n", - " 6.20544581e-01 3.39005049e-01 -3.39005049e-01]\n", - " [ 5.72200270e-02 4.00000000e-02 8.50054327e-01 6.20544581e-01\n", - " 6.20544581e-01 -3.39005049e-01 3.39005049e-01]\n", - " [ 1.13181340e-01 2.67785794e-16 9.18700750e-01 0.00000000e+00\n", - " 8.77582562e-01 0.00000000e+00 4.79425539e-01]\n", - " [ 2.89890247e-01 2.67785794e-16 8.05237266e-01 -8.10780567e-01\n", - " -1.83468211e-01 4.42931442e-01 3.35836307e-01]\n", - " [ 1.94804025e-01 2.67785794e-16 8.66291427e-01 -4.43577140e-01\n", - " 7.03040009e-01 5.50671700e-01 7.57281032e-02]\n", - " [ 3.03197392e-01 -5.51543927e-02 8.62237905e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 2.43597439e-01 5.51543927e-02 7.69416477e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 0.00000000e+00 5.00000000e-01 0.00000000e+00 1.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00]\n", - " [ 0.00000000e+00 5.00000000e-01 0.00000000e+00 1.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00]\n", - " [ 0.00000000e+00 5.00000000e-01 0.00000000e+00 1.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00]\n", - " [ 0.00000000e+00 5.00000000e-01 3.33000000e-01 1.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00]\n", - " [ 0.00000000e+00 5.00000000e-01 3.33000000e-01 1.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00]\n", - " [ 0.00000000e+00 5.00000000e-01 3.33000000e-01 1.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00]\n", - " [ 0.00000000e+00 5.00000000e-01 3.33000000e-01 1.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00]\n", - " [ 0.00000000e+00 5.00000000e-01 3.33000000e-01 7.07106781e-01\n", - " -7.07106781e-01 0.00000000e+00 0.00000000e+00]\n", - " [ 0.00000000e+00 5.00000000e-01 3.33000000e-01 6.20544581e-01\n", - " -6.20544581e-01 -3.39005049e-01 -3.39005049e-01]\n", - " [ 0.00000000e+00 5.00000000e-01 3.33000000e-01 6.20544581e-01\n", - " -6.20544581e-01 -3.39005049e-01 -3.39005049e-01]\n", - " [ 0.00000000e+00 5.00000000e-01 3.33000000e-01 6.20544581e-01\n", - " -6.20544581e-01 -3.39005049e-01 -3.39005049e-01]\n", - " [-2.65904831e-01 5.00000000e-01 5.03735529e-01 8.77582562e-01\n", - " 0.00000000e+00 -4.79425539e-01 0.00000000e+00]\n", - " [-2.65904831e-01 5.00000000e-01 5.03735529e-01 8.77582562e-01\n", - " 0.00000000e+00 -4.79425539e-01 0.00000000e+00]\n", - " [-2.65904831e-01 5.00000000e-01 5.03735529e-01 8.77582562e-01\n", - " 0.00000000e+00 -4.79425539e-01 0.00000000e+00]\n", - " [-2.65904831e-01 5.00000000e-01 5.03735529e-01 8.77582562e-01\n", - " 0.00000000e+00 -4.79425539e-01 0.00000000e+00]\n", - " [-2.21329891e-01 5.00000000e-01 5.73156885e-01 6.20544581e-01\n", - " 6.20544581e-01 -3.39005049e-01 3.39005049e-01]\n", - " [-2.21329891e-01 5.00000000e-01 5.73156885e-01 6.20544581e-01\n", - " 6.20544581e-01 3.39005049e-01 -3.39005049e-01]\n", - " [-2.21329891e-01 5.00000000e-01 5.73156885e-01 6.20544581e-01\n", - " 6.20544581e-01 3.39005049e-01 -3.39005049e-01]\n", - " [-2.21329891e-01 5.00000000e-01 5.73156885e-01 6.20544581e-01\n", - " 6.20544581e-01 3.39005049e-01 -3.39005049e-01]\n", - " [ 5.72200270e-02 5.00000000e-01 8.50054327e-01 8.77582562e-01\n", - " 0.00000000e+00 4.79425539e-01 0.00000000e+00]\n", - " [ 5.72200270e-02 5.00000000e-01 8.50054327e-01 8.77582562e-01\n", - " 0.00000000e+00 4.79425539e-01 0.00000000e+00]\n", - " [ 5.72200270e-02 5.00000000e-01 8.50054327e-01 8.77582562e-01\n", - " 0.00000000e+00 4.79425539e-01 0.00000000e+00]\n", - " [ 5.72200270e-02 5.00000000e-01 8.50054327e-01 8.77582562e-01\n", - " 0.00000000e+00 4.79425539e-01 0.00000000e+00]\n", - " [ 5.72200270e-02 5.00000000e-01 8.50054327e-01 6.20544581e-01\n", - " 6.20544581e-01 3.39005049e-01 -3.39005049e-01]\n", - " [ 5.72200270e-02 5.00000000e-01 8.50054327e-01 6.20544581e-01\n", - " 6.20544581e-01 -3.39005049e-01 3.39005049e-01]\n", - " [ 5.72200270e-02 5.00000000e-01 8.50054327e-01 6.20544581e-01\n", - " 6.20544581e-01 -3.39005049e-01 3.39005049e-01]\n", - " [ 5.72200270e-02 5.00000000e-01 8.50054327e-01 6.20544581e-01\n", - " 6.20544581e-01 -3.39005049e-01 3.39005049e-01]\n", - " [ 1.04766630e-01 5.00000000e-01 9.24103773e-01 0.00000000e+00\n", - " 8.77582562e-01 0.00000000e+00 4.79425539e-01]\n", - " [ 1.04766630e-01 5.00000000e-01 9.24103773e-01 0.00000000e+00\n", - " 8.77582562e-01 0.00000000e+00 4.79425539e-01]\n", - " [ 1.04766630e-01 5.00000000e-01 9.24103773e-01 0.00000000e+00\n", - " 8.77582562e-01 0.00000000e+00 4.79425539e-01]\n", - " [ 1.04766630e-01 5.00000000e-01 9.24103773e-01 0.00000000e+00\n", - " 8.77582562e-01 0.00000000e+00 4.79425539e-01]\n", - " [ 1.94804025e-01 5.00000000e-01 8.66291427e-01 0.00000000e+00\n", - " 8.77582562e-01 0.00000000e+00 4.79425539e-01]\n", - " [ 1.94804025e-01 5.00000000e-01 8.66291427e-01 0.00000000e+00\n", - " 8.77582562e-01 0.00000000e+00 4.79425539e-01]\n", - " [ 1.94804025e-01 5.00000000e-01 8.66291427e-01 0.00000000e+00\n", - " 8.77582562e-01 0.00000000e+00 4.79425539e-01]\n", - " [ 1.94804025e-01 5.00000000e-01 8.66291427e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 1.94804025e-01 5.00000000e-01 8.66291427e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 1.94804025e-01 5.00000000e-01 8.66291427e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 1.94804025e-01 5.00000000e-01 8.66291427e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 2.43945931e-01 5.00000000e-01 8.34737772e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 2.43945931e-01 5.00000000e-01 8.34737772e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 2.63048480e-01 4.64644620e-01 8.64488229e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 2.24843382e-01 5.35355380e-01 8.04987314e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 2.63048480e-01 4.64644620e-01 8.64488229e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 2.24843382e-01 5.35355380e-01 8.04987314e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 2.63048480e-01 4.64644620e-01 8.64488229e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 2.24843382e-01 5.35355380e-01 8.04987314e-01 -4.42931549e-01\n", - " 3.35835838e-01 -8.10780761e-01 1.83467955e-01]\n", - " [-4.00000000e-02 5.00000000e-01 3.00000000e-02 7.07106781e-01\n", - " 0.00000000e+00 7.07106781e-01 0.00000000e+00]\n", - " [-2.00000000e-01 3.80000000e-01 0.00000000e+00 7.07106781e-01\n", - " 0.00000000e+00 7.07106781e-01 0.00000000e+00]\n", - " [ 0.00000000e+00 5.00000000e-01 1.83000000e-01 1.00000000e+00\n", - " 0.00000000e+00 0.00000000e+00 0.00000000e+00]\n", - " [-1.39684183e-01 5.00000000e-01 4.22690183e-01 8.77582562e-01\n", - " 0.00000000e+00 -4.79425539e-01 0.00000000e+00]\n", - " [-1.11074170e-01 5.20000000e-01 7.41993865e-01 8.77582562e-01\n", - " 0.00000000e+00 4.79425539e-01 0.00000000e+00]\n", - " [ 0.00000000e+00 5.00000000e-01 3.33000000e-01 6.20544581e-01\n", - " -6.20544581e-01 -3.39005049e-01 -3.39005049e-01]\n", - " [-2.21329891e-01 5.00000000e-01 5.73156885e-01 6.20544581e-01\n", - " 6.20544581e-01 3.39005049e-01 -3.39005049e-01]\n", - " [ 5.72200270e-02 5.40000000e-01 8.50054327e-01 6.20544581e-01\n", - " 6.20544581e-01 -3.39005049e-01 3.39005049e-01]\n", - " [ 1.13181340e-01 5.00000000e-01 9.18700750e-01 0.00000000e+00\n", - " 8.77582562e-01 0.00000000e+00 4.79425539e-01]\n", - " [ 2.89890247e-01 5.00000000e-01 8.05237266e-01 -8.10780567e-01\n", - " -1.83468211e-01 4.42931442e-01 3.35836307e-01]\n", - " [ 1.94804025e-01 5.00000000e-01 8.66291427e-01 -4.43577140e-01\n", - " 7.03040009e-01 5.50671700e-01 7.57281032e-02]\n", - " [ 3.03197392e-01 4.44845607e-01 8.62237905e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]\n", - " [ 2.43597439e-01 5.55154393e-01 7.69416477e-01 1.83467955e-01\n", - " 8.10780761e-01 3.35835838e-01 4.42931549e-01]]\n" - ] - } - ], - "source": [ - "X0 = C.getFrameState()\n", - "print('frame state: ', X0)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Let's do a very questionable thing: adding .1 to all numbers in the frame matrix!" - ] - }, - { - "cell_type": "code", - "execution_count": 17, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 17, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "X = X0 + .1\n", - "C.setFrameState(X)\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "That totally broke the original design of the robot! Setting global frame states overwrites the relative transformations between frames.\n", - "\n", - "(Also, the rows of X have non-normalized quaternions! These are normalized when setting the frame state.)\n", - "\n", - "Let's reset:" - ] - }, - { - "cell_type": "code", - "execution_count": 18, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 18, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C.setFrameState(X0)\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - " ## Selecting joints\n", - "\n", - "Often one would like to choose which joints are actually active, that is, which joints are referred to in q. This allows one to sub-select joints and work only with projections of the full configuration state. This changes the joint state dimensionality, including ordering of entries in q. The frame state is not affected by such a selection of active joints." - ] - }, - { - "cell_type": "code", - "execution_count": 19, - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "joint state: [0.]\n", - "joint names: ['panda_joint1']\n" - ] - } - ], - "source": [ - "C.selectJoints(['panda_joint1', 'panda_joint1'])\n", - "print('joint state: ', C.getJointState())\n", - "print('joint names: ', C.getJointNames() )" - ] - }, - { - "cell_type": "code", - "execution_count": 20, - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "joint state: [ 0. -1.00001 0. -2.00001 0. 2.00001 0. 0.05\n", - " 0. -1.00001 0. -2.00001 0. 2.00001 0. 0.05 ]\n", - "joint names: ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7', 'panda_finger_joint1panda_finger_joint2', 'r_panda_joint1', 'r_panda_joint2', 'r_panda_joint3', 'r_panda_joint4', 'r_panda_joint5', 'r_panda_joint6', 'r_panda_joint7', 'r_panda_finger_joint1r_panda_finger_joint2']\n" - ] - } - ], - "source": [ - "C.selectJoints([], True)\n", - "print('joint state: ', C.getJointState())\n", - "print('joint names: ', C.getJointNames() )" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## View interaction and releasing objects" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "You can close and re-open the view window" - ] - }, - { - "cell_type": "code", - "execution_count": 21, - "metadata": {}, - "outputs": [], - "source": [ - "C.view_close()" - ] - }, - { - "cell_type": "code", - "execution_count": 22, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 22, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "# things are still there\n", - "C.view(pause=False, message='this is a message')" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "For user interaction it is often useful to wait for a keypress (making `view` a blocking call):" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "keypressed = C.view(True, 'press some key!')\n", - "print('pressed key:', keypressed, chr(keypressed))" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Get a screenshot:" - ] - }, - { - "cell_type": "code", - "execution_count": 23, - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - " (400, 400, 3)\n" - ] - } - ], - "source": [ - "img = C.view_getScreenshot()\n", - "print(type(img), img.shape)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "And release everything, including closing the view" - ] - }, - { - "cell_type": "code", - "execution_count": 24, - "metadata": {}, - "outputs": [], - "source": [ - "del C" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3 (ipykernel)", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 4 -} diff --git a/notebooks/1b-botop.ipynb b/notebooks/1b-botop.ipynb deleted file mode 100644 index a0a1da5d6..000000000 --- a/notebooks/1b-botop.ipynb +++ /dev/null @@ -1,483 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "e193f01d", - "metadata": {}, - "source": [ - "# Robot Operation (BotOp) interface\n", - "\n", - "This describes basics to interact with a real or simulated robot. The BotOp (=robot operation) interface is very narrow. The move methods set/overwrite a spline reference for the robot. (Also compliance around the reference can be set.) The gripper methods operate grippers. The getImage.. methods grab images or point clouds from the camera.\n", - "\n", - "This interface different to a more *generic physical simulation* interface. If you're interested in the latter (e.g. to implement a gym environment) look at the `Simulation` tutorial. The simulation used here is a real-time threaded process that mimics the specific BotOp interface -- to make it swappable with a real robot.\n", - "\n", - "The simulation can be run in many different modes: pure kinematic (no physics for objects), a physics simulator with physics for objects but still kinematic robot, a physic simulator with PD motors for the robot." - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "id": "31a434d3", - "metadata": {}, - "outputs": [], - "source": [ - "from robotic import ry\n", - "import numpy as np\n", - "import time" - ] - }, - { - "cell_type": "markdown", - "id": "74bb70b4", - "metadata": {}, - "source": [ - "ry has global parameters, that can be defined in `rai.cfg` or with the following calls.\n", - "The simulation behaves very differently depending on `botim/engine` [physx or kinematic] and `multibody`" - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "id": "bb4031b4", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "-- ry.cpp:operator():99(0) python,\n", - "message: \"Hello, the local 'rai.cfg' was loaded\",\n", - "botsim/verbose: 2,\n", - "physx/motorKp: 10000,\n", - "physx/motorKd: 1000,\n", - "botsim/engine: physx,\n", - "physx/multibody: 1\n" - ] - } - ], - "source": [ - "ry.params_add({'botsim/verbose': 2., 'physx/motorKp': 10000., 'physx/motorKd': 1000.})\n", - "ry.params_add({'botsim/engine': 'physx'}) #makes a big difference!\n", - "ry.params_add({'physx/multibody': True}) #makes a big difference!\n", - "ry.params_print()" - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "id": "d1bff41b", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 3, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C = ry.Config()\n", - "C.addFile(ry.raiPath('../rai-robotModels/scenarios/pandaSingle.g'))\n", - "C.view(False, 'this is your workspace data structure C -- NOT THE SIMULTATION')" - ] - }, - { - "cell_type": "markdown", - "id": "2333c4b1", - "metadata": {}, - "source": [ - "We open a robot interface in simulation (False). True would directly open communication to one or two pandas (depending no how many are defined in C). The `botsim/verbose` above leads to the explicit verbosity when creating the simulator interface." - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "id": "6832eb10", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "-- kin_physx.cpp:PhysXInterface:768(0) starting PhysX engine ...\n", - "-- kin_physx.cpp:addGround:238(0) ... done starting PhysX engine\n", - "-- kin_physx.cpp:addGround:239(0) creating Configuration within PhysX ...\n", - "-- kin_physx.cpp:addLink:254(0) adding link 'world' as static with 1 shapes\n", - " table\n", - "-- kin_physx.cpp:addMultiBody:466(0) adding multibody with base 'l_panda_base' with the following links ...\n", - "-- kin_physx.cpp:addMultiBody:486(0) adding multibody link 'l_panda_base' as kinematic with 1 shapes\n", - " l_panda_link0_0\n", - "-- kin_physx.cpp:addMultiBody:486(0) adding multibody link 'l_panda_joint1' as dynamic with 1 shapes\n", - " l_panda_link1_0\n", - "-- kin_physx.cpp:addMultiBody:486(0) adding multibody link 'l_panda_joint2' as dynamic with 2 shapes\n", - " l_panda_link2_0 bellybutton\n", - "-- kin_physx.cpp:addMultiBody:486(0) adding multibody link 'l_panda_joint3' as dynamic with 1 shapes\n", - " l_panda_link3_0\n", - "-- kin_physx.cpp:addMultiBody:486(0) adding multibody link 'l_panda_joint4' as dynamic with 1 shapes\n", - " l_panda_link4_0\n", - "-- kin_physx.cpp:addMultiBody:486(0) adding multibody link 'l_panda_joint5' as dynamic with 1 shapes\n", - " l_panda_link5_0\n", - "-- kin_physx.cpp:addMultiBody:486(0) adding multibody link 'l_panda_joint6' as dynamic with 1 shapes\n", - " l_panda_link6_0\n", - "-- kin_physx.cpp:addMultiBody:486(0) adding multibody link 'l_panda_joint7' as dynamic with 2 shapes\n", - " l_panda_link7_0 l_panda_hand_0\n", - "-- kin_physx.cpp:addMultiBody:486(0) adding multibody link 'l_panda_finger_joint1' as dynamic with 1 shapes\n", - " l_panda_leftfinger_0\n", - "-- kin_physx.cpp:addMultiBody:486(0) adding multibody link 'l_panda_finger_joint2' as dynamic with 1 shapes\n", - " l_panda_rightfinger_0\n", - "-- kin_physx.cpp:addMultiBody:592(0) ... done with multibody with base 'l_panda_base'\n", - "-- kin_physx.cpp:PhysXInterface:805(0) ... done creating Configuration within PhysX\n" - ] - } - ], - "source": [ - "bot = ry.BotOp(C, False)\n", - "#note that in sim, when physx multibody is activated, arms are going down! free floating..." - ] - }, - { - "cell_type": "markdown", - "id": "c4ad9bb7", - "metadata": {}, - "source": [ - "We define 2 reference poses, q0=home and q1=(2nd joint bend), so that we can move back and forth between them" - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "id": "afe800f7", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "[ 0. -0.5 0. -2. 0. 2. -0.5] [ 0. -0.3 0. -2. 0. 2. -0.5]\n" - ] - } - ], - "source": [ - "qHome = bot.get_qHome()\n", - "q0 = qHome.copy()\n", - "q1 = q0.copy()\n", - "q1[1] = q1[1] + .2\n", - "print(q0, q1)" - ] - }, - { - "cell_type": "markdown", - "id": "86f72e9b", - "metadata": {}, - "source": [ - "The `moveTo` is the simplest way to move the robot from current to target. It internally creates a spline to the target with optimal timing and follows it. The call is *non-blocking*. Also, your workspace config C is not kept in sync with the real/sim. If you want to wait till the motion is finished, you need to do manually checking the /time_til_end_of_reference_spline/, and meanwhile staying sync'ed." - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "id": "443856f3", - "metadata": {}, - "outputs": [], - "source": [ - "bot.moveTo(q1)\n", - "\n", - "while bot.getTimeToEnd()>0:\n", - " bot.sync(C, .1)" - ] - }, - { - "cell_type": "markdown", - "id": "fa41eca8", - "metadata": {}, - "source": [ - "The internal spline reference can be appended: As `moveTo` is non-blocking, you can append several moves like this:" - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "id": "182b64dd", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "timeToEnd: -0.073524058876411\n", - "timeToEnd: 1.0957189420649807\n", - "timeToEnd: 2.191437884129961\n" - ] - } - ], - "source": [ - "print('timeToEnd:', bot.getTimeToEnd())\n", - "bot.moveTo(q0)\n", - "print('timeToEnd:', bot.getTimeToEnd())\n", - "bot.moveTo(q1)\n", - "print('timeToEnd:', bot.getTimeToEnd())\n", - "bot.moveTo(q0)\n", - "\n", - "while bot.getTimeToEnd()>0:\n", - " bot.sync(C, .1)" - ] - }, - { - "cell_type": "markdown", - "id": "abc71ed9", - "metadata": {}, - "source": [ - "Setting splines becomes reactive, when we can smoothly overwrite the spline reference with high frequency. Let's create a randomly moving target object and track it." - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "id": "c3dbe900", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 8, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "#this reference frame only appears in your workspace C - not the simulation!\n", - "target = C.addFrame('target', 'table')\n", - "target.setShape(ry.ST.marker, [.1])\n", - "target.setRelativePosition([0., .3, .3])\n", - "pos = target.getPosition()\n", - "cen = pos.copy()\n", - "C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": 9, - "id": "b2a168d1", - "metadata": {}, - "outputs": [], - "source": [ - "# you'll learn about KOMO later - this defines a basic Inverse Kinematics method\n", - "def IK(C, pos):\n", - " q0 = C.getJointState()\n", - " komo = ry.KOMO(C, 1, 1, 0, False) #one phase one time slice problem, with 'delta_t=1', order=0\n", - " komo.addObjective([], ry.FS.jointState, [], ry.OT.sos, [1e-1], q0) #cost: close to 'current state'\n", - " komo.addObjective([], ry.FS.jointState, [], ry.OT.sos, [1e-1], qHome) #cost: close to qHome\n", - " komo.addObjective([], ry.FS.positionDiff, ['l_gripper', 'target'], ry.OT.eq, [1e1]) #constraint: gripper position\n", - " \n", - " ret = ry.NLP_Solver(komo.nlp(), verbose=0) .solve()\n", - " \n", - " return [komo.getPath()[0], ret]" - ] - }, - { - "cell_type": "markdown", - "id": "76e13a25", - "metadata": {}, - "source": [ - "The following is just 'setting' the workspace C to the IK solution - no motion send to the real/robot:" - ] - }, - { - "cell_type": "code", - "execution_count": 10, - "id": "4998d869", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "{ time: 0.005057, evals: 6, done: 1, feasible: 1, sos: 0.00789171, f: 0, ineq: 0, eq: 0.00138392 }\n", - "{ time: 0.001435, evals: 4, done: 1, feasible: 1, sos: 0.0055078, f: 0, ineq: 0, eq: 0.00038073 }\n", - "{ time: 0.001453, evals: 3, done: 1, feasible: 1, sos: 0.00485786, f: 0, ineq: 0, eq: 0.0031066 }\n", - "{ time: 0.000382, evals: 3, done: 1, feasible: 1, sos: 0.00552496, f: 0, ineq: 0, eq: 0.00285066 }\n", - "{ time: 0.000871, evals: 4, done: 1, feasible: 1, sos: 0.00481289, f: 0, ineq: 0, eq: 0.000176548 }\n", - "{ time: 0.000181, evals: 3, done: 1, feasible: 1, sos: 0.00416384, f: 0, ineq: 0, eq: 0.00404598 }\n", - "{ time: 0.001811, evals: 3, done: 1, feasible: 1, sos: 0.00394648, f: 0, ineq: 0, eq: 0.00118522 }\n", - "{ time: 0.000637, evals: 3, done: 1, feasible: 1, sos: 0.00410849, f: 0, ineq: 0, eq: 0.00429565 }\n", - "{ time: 0.001591, evals: 4, done: 1, feasible: 1, sos: 0.00454469, f: 0, ineq: 0, eq: 0.000211266 }\n", - "{ time: 0.000784, evals: 3, done: 1, feasible: 1, sos: 0.00470588, f: 0, ineq: 0, eq: 0.00357698 }\n", - "{ time: 0.000942, evals: 3, done: 1, feasible: 1, sos: 0.00478467, f: 0, ineq: 0, eq: 0.00148874 }\n", - "{ time: 0.001108, evals: 3, done: 1, feasible: 1, sos: 0.00457452, f: 0, ineq: 0, eq: 0.00162474 }\n", - "{ time: 0.001451, evals: 4, done: 1, feasible: 1, sos: 0.005894, f: 0, ineq: 0, eq: 0.000439926 }\n", - "{ time: 0.002882, evals: 3, done: 1, feasible: 1, sos: 0.00540869, f: 0, ineq: 0, eq: 0.0027726 }\n", - "{ time: 0.001624, evals: 4, done: 1, feasible: 1, sos: 0.00720508, f: 0, ineq: 0, eq: 0.00069492 }\n", - "{ time: 0.001267, evals: 4, done: 1, feasible: 1, sos: 0.0075089, f: 0, ineq: 0, eq: 0.000505518 }\n", - "{ time: 0.002128, evals: 3, done: 1, feasible: 1, sos: 0.0078162, f: 0, ineq: 0, eq: 0.0019024 }\n", - "{ time: 0.001489, evals: 3, done: 1, feasible: 1, sos: 0.00831903, f: 0, ineq: 0, eq: 0.00241169 }\n", - "{ time: 0.005757, evals: 4, done: 1, feasible: 1, sos: 0.0106921, f: 0, ineq: 0, eq: 0.000467631 }\n", - "{ time: 0.001208, evals: 4, done: 1, feasible: 1, sos: 0.0115667, f: 0, ineq: 0, eq: 0.000478951 }\n" - ] - } - ], - "source": [ - "for t in range(20):\n", - " time.sleep(.1)\n", - " pos = cen + .98 * (pos-cen) + 0.02 * np.random.randn(3)\n", - " target.setPosition(pos)\n", - " \n", - " q_target, ret = IK(C, pos)\n", - " print(ret)\n", - " C.setJointState(q_target)\n", - " C.view()" - ] - }, - { - "cell_type": "markdown", - "id": "c0d79cae", - "metadata": {}, - "source": [ - "We now generate reative motion by smoothly overwriting the spline reference. Increasing time cost makes it more agressive (penalized total duration of estimated cubic spline)." - ] - }, - { - "cell_type": "code", - "execution_count": 11, - "id": "c5af1933", - "metadata": {}, - "outputs": [], - "source": [ - "for t in range(100):\n", - " bot.sync(C, .1) #keep the workspace C sync'ed to real/sim, and idle .1 sec\n", - " pos = cen + .98 * (pos-cen) + 0.02 * np.random.randn(3)\n", - " target.setPosition(pos)\n", - " \n", - " q_target, ret = IK(C, pos)\n", - " bot.moveTo(q_target, timeCost=5., overwrite=True)" - ] - }, - { - "cell_type": "markdown", - "id": "35e22aa1", - "metadata": {}, - "source": [ - "Good practise is to always allow a user aborting motion execution. In this example, key 'q' will break the loop and call a home() (which is the same as moveTo(qHome, 1., True)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "09468d0c", - "metadata": {}, - "outputs": [], - "source": [ - "for t in range(5):\n", - " bot.moveTo(q1)\n", - " bot.wait(C) #same as 'loop sync til keypressed or endOfTime', but also raises user window\n", - " if bot.getKeyPressed()==ord('q'):\n", - " break;\n", - " \n", - " bot.moveTo(q0)\n", - " bot.wait(C)\n", - " if bot.getKeyPressed()==ord('q'):\n", - " break;\n", - "\n", - "bot.home(C)" - ] - }, - { - "cell_type": "markdown", - "id": "3776fd7a", - "metadata": {}, - "source": [ - "gripper movements also do not block:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "9b62c7c5", - "metadata": {}, - "outputs": [], - "source": [ - "bot.gripperMove(ry._left, width=.02)\n", - "\n", - "while not bot.gripperDone(ry._left):\n", - " bot.sync(C, .1)\n", - "\n", - "bot.gripperMove(ry._left, width=.075)\n", - "\n", - "while not bot.gripperDone(ry._left):\n", - " bot.sync(C, .1)" - ] - }, - { - "cell_type": "markdown", - "id": "2dfbfcea", - "metadata": {}, - "source": [ - "Always close the bot/sim properly:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "2ea154ed", - "metadata": {}, - "outputs": [], - "source": [ - "del bot\n", - "del C" - ] - }, - { - "cell_type": "markdown", - "id": "ac37869b", - "metadata": {}, - "source": [ - "As a side note, we can always check which global config parameters have been queried by the code so far. That gives an idea of which global parameters exist:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "24342be8", - "metadata": {}, - "outputs": [], - "source": [ - "ry.params_print()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "6c9473f1", - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3 (ipykernel)", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/notebooks/1c-komo.ipynb b/notebooks/1c-komo.ipynb deleted file mode 100644 index 52c5669c1..000000000 --- a/notebooks/1c-komo.ipynb +++ /dev/null @@ -1,726 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "dffc232a", - "metadata": {}, - "source": [ - "# KOMO: Motion Optimization\n", - "\n", - "KOMO is a framework for designing motion by formulating optimization problems. Inverse kinematics (IK) is the special case of optimizing only over a single configuration rather than a path. Formulating KOMO problems is key to realizing motion in `rai`.\n", - "\n", - "This tutorial shows basics on how IK, rough waypoint optimization, and fine path optimization can be formulated as non-linear mathematical program (NLP) using KOMO. Essentially, the `addObjective` allows to add costs or constraints over any `Feature` to the NLP (same features that can be evaluated with 'C.eval')." - ] - }, - { - "cell_type": "markdown", - "id": "a177972b", - "metadata": {}, - "source": [ - "## Minimal IK example" - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "id": "8e07bf36", - "metadata": {}, - "outputs": [], - "source": [ - "from robotic import ry\n", - "import numpy as np\n", - "import time" - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "id": "059a8ee7", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 2, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C = ry.Config()\n", - "C.addFile(ry.raiPath('scenarios/pandaSingle.g'))\n", - "C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "id": "582b68ba", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 3, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C.addFrame('box') \\\n", - " .setPosition([-.25,.1,1.]) \\\n", - " .setShape(ry.ST.ssBox, size=[.06,.06,.06,.005]) \\\n", - " .setColor([1,.5,0]) \\\n", - " .setContact(True)\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "id": "ac059dc2", - "metadata": {}, - "source": [ - "The following defines an optimization problem over a single configuration. The KOMO object essentially contains (1) copies of the configuration(s) over which we optimize, and (2) the list of objectives (=costs & constraints) that define the optimization problem.\n", - "\n", - "The constructor declares over how many configurations (single, waypoints, path..) we optimize. The addObjective methods add costs or constraints:" - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "id": "bccb7b55", - "metadata": {}, - "outputs": [], - "source": [ - "qHome = C.getJointState()\n", - "komo = ry.KOMO(C, 1, 1, 0, False)\n", - "komo.addObjective(times=[], feature=ry.FS.jointState, frames=[], type=ry.OT.sos, scale=[1e-1], target=qHome);\n", - "komo.addObjective([], ry.FS.positionDiff, ['l_gripper', 'box'], ry.OT.eq, [1e1]);" - ] - }, - { - "cell_type": "markdown", - "id": "76895850", - "metadata": {}, - "source": [ - "We explain the KOMO constructor arguments later. (The above defines an IK problem.)\n", - "\n", - "The `addObjective` method has signature\n", - "* times: the time intervals (subset of configurations in a path) over which this feature is active (irrelevant for IK)\n", - "* feature: the feature symbol (see advanced `Feature` tutorial)\n", - "* frames: the frames for which the feature is computed, given as list of frame names\n", - "* type: whether this is a sum-of-squares (sos) cost, or eq or ineq constraint\n", - "* scale: the matrix(!) by which the feature is multiplied\n", - "* target: the offset which is substracted from the feature (before scaling)\n", - "\n", - "Please see more formal details " - ] - }, - { - "cell_type": "markdown", - "id": "9e27cfa8", - "metadata": {}, - "source": [ - "Given this definition of an optimization problem, we can call a generic NLP solver:" - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "id": "178e3d42", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "{ time: 0.001224, evals: 6, done: 1, feasible: 1, sos: 0.00414146, f: 0, ineq: 0, eq: 0.00188382 }====nlp==== method:AugmentedLagrangian bounded: yes\n", - "==nlp== it:0 evals:0 mu:1 nu:1 muLB:0.1\n", - "----newton---- initial point f(x):16.0447 alpha:1 beta:1\n", - "--newton-- it: 1 |Delta|: 0.2 alpha: 1 evals: 2 f(y): 6.55808 ACCEPT\n", - "--newton-- it: 2 |Delta|: 0.2 alpha: 1 evals: 3 f(y): 0.686083 ACCEPT\n", - "--newton-- it: 3 |Delta|: 0.144223 alpha: 1 evals: 4 f(y): 0.0170221 ACCEPT\n", - "--newton-- it: 4 |Delta|: 0.0221449 alpha: 1 evals: 5 f(y): 0.00418093 ACCEPT\n", - "--newton-- stopping: 'absMax(Delta) 1\n" - ] - } - ], - "source": [ - "q = komo.getPath()\n", - "print(type(q), len(q))" - ] - }, - { - "cell_type": "markdown", - "id": "9f92e896", - "metadata": {}, - "source": [ - "We're done with KOMO and can destroy it. Then set the optimal joint state in C and view it:" - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "id": "b20fc581", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 8, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "del komo #also closes komo view\n", - "C.setJointState(q[0])\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "id": "57ccf739", - "metadata": {}, - "source": [ - "## Example for more constraints: box grasping IK\n", - "\n", - "The key to design motions is to add clever constraints. Here is an example for more realistic box grasping:" - ] - }, - { - "cell_type": "code", - "execution_count": 9, - "id": "bdbbbe7b", - "metadata": {}, - "outputs": [], - "source": [ - "komo = ry.KOMO(C, 1,1,0, True)\n", - "komo.addObjective([], ry.FS.jointState, [], ry.OT.sos, [1e-1], qHome)\n", - "komo.addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.eq)\n", - "komo.addObjective([], ry.FS.jointLimits, [], ry.OT.ineq)\n", - "komo.addObjective([], ry.FS.positionDiff, ['l_gripper', 'box'], ry.OT.eq, [1e1])\n", - "komo.addObjective([], ry.FS.scalarProductXX, ['l_gripper', 'box'], ry.OT.eq, [1e1], [0])\n", - "komo.addObjective([], ry.FS.scalarProductXZ, ['l_gripper', 'box'], ry.OT.eq, [1e1], [0])\n", - "komo.addObjective([], ry.FS.distance, ['l_palm', 'box'], ry.OT.ineq, [1e1])" - ] - }, - { - "cell_type": "markdown", - "id": "de8fe5b5", - "metadata": {}, - "source": [ - "The two `scalarProduct` feature state that the gripper x-axis (which is the axis connecting the fingers) should be orthogonal to the object x- and z-axes. That implies fingers to normally oppose the object's y-planes.\n", - "\n", - "Note that grasping could also be opposing the object x- or z- planes -- see below." - ] - }, - { - "cell_type": "code", - "execution_count": 10, - "id": "dab4fbee", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "{ time: 0.00086, evals: 4, done: 1, feasible: 1, sos: 0.00552548, f: 0, ineq: 0, eq: 0.00124449 }\n", - "-- Always check feasibility flag of NLP solver return\n" - ] - } - ], - "source": [ - "ret = ry.NLP_Solver(komo.nlp(), verbose=0 ) .solve()\n", - "print(ret)\n", - "if ret.feasible:\n", - " print('-- Always check feasibility flag of NLP solver return')\n", - "else:\n", - " print('-- THIS IS INFEASIBLE!')" - ] - }, - { - "cell_type": "code", - "execution_count": 11, - "id": "f1970bb1", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 11, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "q = komo.getPath()\n", - "C.setJointState(q[0])\n", - "C.view(False, \"IK solution\")" - ] - }, - { - "cell_type": "markdown", - "id": "bef1a139", - "metadata": {}, - "source": [ - "Reusing the KOMO instance is ok if some aspect of the configuration changes and you want to resolve the same problem:" - ] - }, - { - "cell_type": "code", - "execution_count": 12, - "id": "a2d386d6", - "metadata": {}, - "outputs": [], - "source": [ - "box = C.getFrame('box')\n", - "box.setPosition([-.25,.1,1.])\n", - "p0 = box.getPosition()" - ] - }, - { - "cell_type": "code", - "execution_count": 13, - "id": "67ef81d4", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "{ time: 0.001093, evals: 6, done: 1, feasible: 1, sos: 0.00142469, f: 0, ineq: 0, eq: 0.00116721 }\n", - "{ time: 0.004594, evals: 33, done: 1, feasible: 1, sos: 0.0293914, f: 0, ineq: 0, eq: 0.000336433 }\n", - "{ time: 0.00941, evals: 12, done: 1, feasible: 1, sos: 0.0439537, f: 0, ineq: 0, eq: 0.00158138 }\n", - "{ time: 0.038425, evals: 131, done: 1, feasible: 1, sos: 0.156179, f: 0, ineq: 0, eq: 0.337268 }\n", - "{ time: 0.017076, evals: 55, done: 1, feasible: 0, sos: 0.173381, f: 0, ineq: 0, eq: 2.46147 }\n", - "{ time: 0.023718, evals: 86, done: 1, feasible: 1, sos: 0.0803936, f: 0, ineq: 0, eq: 0.000521253 }\n", - "{ time: 0.007224, evals: 28, done: 1, feasible: 1, sos: 0.158211, f: 0, ineq: 0, eq: 0.0010477 }\n", - "{ time: 0.01382, evals: 100, done: 1, feasible: 1, sos: 0.0104285, f: 0, ineq: 0, eq: 4.74187e-05 }\n", - "{ time: 0.004671, evals: 11, done: 1, feasible: 1, sos: 0.0278131, f: 0, ineq: 0, eq: 0.000474963 }\n", - "{ time: 0.001154, evals: 6, done: 1, feasible: 1, sos: 0.0125726, f: 0, ineq: 0, eq: 0.00127309 }\n" - ] - } - ], - "source": [ - "for t in range(10):\n", - " box.setPosition(p0 + .2 * np.random.randn(3))\n", - " komo.updateRootObjects(C) #only works for root object (the 'box' is one)\n", - " ret = ry.NLP_Solver(komo.nlp(), verbose=0 ) .solve()\n", - " print(ret)\n", - " q = komo.getPath()\n", - " C.setJointState(q[0])\n", - " C.view(False, 'IK solution - ' + ('*** INFEASIBLE ***' if not ret.feasible else 'feasible'))\n", - " time.sleep(1.)" - ] - }, - { - "cell_type": "markdown", - "id": "c0e78a35", - "metadata": {}, - "source": [ - "So the solver finds feasible grasps and exploits the null space of the constraints (grasps from different directions, but always opposing the y-planes).\n", - "\n", - "To make this proper, we should actually test all three possible grasps - so let's define 3 IK problems, solve each, and pick the best:" - ] - }, - { - "cell_type": "code", - "execution_count": 14, - "id": "8222658d", - "metadata": {}, - "outputs": [], - "source": [ - "del komo\n", - "komo = []\n", - "for k in range(3):\n", - " komo.append(ry.KOMO(C, 1,1,0, True))\n", - " komo[k].addObjective([], ry.FS.jointState, [], ry.OT.sos, [1e-1], qHome)\n", - " komo[k].addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.eq)\n", - " komo[k].addObjective([], ry.FS.jointLimits, [], ry.OT.ineq)\n", - " komo[k].addObjective([], ry.FS.positionDiff, ['l_gripper', 'box'], ry.OT.eq, [1e1])\n", - " komo[k].addObjective([], ry.FS.distance, ['l_palm', 'box'], ry.OT.ineq, [1e1])\n", - "\n", - "komo[0].addObjective([], ry.FS.scalarProductXY, ['l_gripper', 'box'], ry.OT.eq, [1e1], [0])\n", - "komo[0].addObjective([], ry.FS.scalarProductXZ, ['l_gripper', 'box'], ry.OT.eq, [1e1], [0])\n", - "\n", - "komo[1].addObjective([], ry.FS.scalarProductXX, ['l_gripper', 'box'], ry.OT.eq, [1e1], [0])\n", - "komo[1].addObjective([], ry.FS.scalarProductXZ, ['l_gripper', 'box'], ry.OT.eq, [1e1], [0])\n", - "\n", - "komo[2].addObjective([], ry.FS.scalarProductXX, ['l_gripper', 'box'], ry.OT.eq, [1e1], [0])\n", - "komo[2].addObjective([], ry.FS.scalarProductXY, ['l_gripper', 'box'], ry.OT.eq, [1e1], [0])" - ] - }, - { - "cell_type": "code", - "execution_count": 15, - "id": "09c7a057", - "metadata": {}, - "outputs": [], - "source": [ - "for t in range(10):\n", - " box.setPosition(p0 + .2 * np.random.randn(3))\n", - " box.setQuaternion(np.random.randn(4)) #random orientation\n", - " \n", - " score = []\n", - " for k in range(3):\n", - " komo[k].updateRootObjects(C)\n", - " ret = ry.NLP_Solver(komo[k].nlp(), verbose=0 ) .solve()\n", - " score.append( 100.*(ret.eq+ret.ineq) + ret.sos )\n", - " \n", - " k = np.argmin(score)\n", - " C.setJointState(komo[k].getPath()[0])\n", - " C.view(False, f'IK solution {k} - ' + ('*** INFEASIBLE ***' if not ret.feasible else 'feasible'))\n", - " time.sleep(1.)" - ] - }, - { - "cell_type": "code", - "execution_count": 16, - "id": "cda905f5", - "metadata": {}, - "outputs": [], - "source": [ - "del komo\n", - "del C" - ] - }, - { - "cell_type": "markdown", - "id": "f7d69b02", - "metadata": {}, - "source": [ - "## Waypoints example\n", - "\n", - "Motion design can often be done by computing waypoints, i.e. a none-fine-resolution sequence of poses. The BotOp interface can then spline-interpolate between them when executing them.\n", - "\n", - "Let's define a configuration where the desired gripper waypoints are pre-defined as marker frames. (That's a common pattern: Simplify defining constraints by adding helper reference frames in the configuration.)" - ] - }, - { - "cell_type": "code", - "execution_count": 17, - "id": "a6da9bda", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 17, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C = ry.Config()\n", - "C.addFile(ry.raiPath('scenarios/pandaSingle.g'))\n", - "C.addFrame('way1'). setShape(ry.ST.marker, [.1]) .setPosition([.4, .2, 1.])\n", - "C.addFrame('way2'). setShape(ry.ST.marker, [.1]) .setPosition([.4, .2, 1.4])\n", - "C.addFrame('way3'). setShape(ry.ST.marker, [.1]) .setPosition([-.4, .2, 1.])\n", - "C.addFrame('way4'). setShape(ry.ST.marker, [.1]) .setPosition([-.4, .2, 1.4])\n", - "C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": 18, - "id": "7c3a74d2", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "{ time: 0.001438, evals: 10, done: 1, feasible: 1, sos: 2.3949, f: 0, ineq: 0, eq: 0.000292005 }\n", - "[[-0.35333405 -0.05475643 -0.41824617 -2.08348439 -0.05931971 2.17654835\n", - " -0.50101108]\n", - " [-0.29338172 -0.37617078 -0.40548624 -1.73266988 -0.02274141 2.33778584\n", - " -0.50190159]\n", - " [ 0.44235508 -0.06356002 0.31597399 -2.10275629 0.12245479 2.2037305\n", - " -0.50256413]\n", - " [ 0.43491826 -0.36086469 0.27555955 -1.74089886 0.12219269 2.34815424\n", - " -0.50291742]]\n" - ] - } - ], - "source": [ - "komo = ry.KOMO(C, phases=4, slicesPerPhase=1, kOrder=1, enableCollisions=False)\n", - "komo.addControlObjective([], 0, 1e-1)\n", - "komo.addControlObjective([], 1, 1e0)\n", - "komo.addObjective([1], ry.FS.positionDiff, ['l_gripper', 'way1'], ry.OT.eq, [1e1])\n", - "komo.addObjective([2], ry.FS.positionDiff, ['l_gripper', 'way2'], ry.OT.eq, [1e1])\n", - "komo.addObjective([3], ry.FS.positionDiff, ['l_gripper', 'way3'], ry.OT.eq, [1e1])\n", - "komo.addObjective([4], ry.FS.positionDiff, ['l_gripper', 'way4'], ry.OT.eq, [1e1])\n", - "\n", - "ret = ry.NLP_Solver(komo.nlp(), verbose=0 ) .solve()\n", - "print(ret)\n", - "q = komo.getPath()\n", - "print(q)\n", - "\n", - "for t in range(4):\n", - " C.setJointState(q[t])\n", - " C.view(False, f'waypoint {t}')\n", - " time.sleep(1)" - ] - }, - { - "cell_type": "markdown", - "id": "f6263d5c", - "metadata": {}, - "source": [ - "The `KOMO constructor` has arguments:\n", - "* config: the configuration, which is copied once (for IK) or many times (for waypoints/paths) to be the optimization variable\n", - "* phases: the number P of phases (which essentially defines the real-valued interval [0,P] over which objectives can be formulated)\n", - "* slicesPerPhase: the discretizations per phase -> in total we have phases*slicesPerPhases configurations which form the path and over which we optimize\n", - "* kOrder: the \"Markov-order\", i.e., maximal tuple of configurations over which we formulate features (e.g. take finite differences)\n", - "\n", - "In our waypoint case: We have 4 phases, one for each waypoint. We don't sub-sample the motion between waypoints, which is why we have slicesPerPhase=1. We formulate this as a 1-order problem: Some features take the finite difference between consecutive configurations (namely, to penalize velocities).\n", - "\n", - "The `addControlObjective` is /almost/ the same as adding a `FS.jointState` objective: It penalizes distances in joint space. It has three arguments:\n", - "* times: (as for `addObjective`) the phase-interval in which this objective holds; [] means all times\n", - "* order: Do we penalize the jointState directly (order=0: penalizing sqr distance to qHome, order=1: penalizing sqr distances between consecutive configurations (velocities), order=2: penalizing accelerations across 3 configurations)\n", - "* scale: as usual, but modulated by a factor \"sqrt(delta t)\" that somehow ensures total control costs in approximately independent of the choice of stepsPerPhase\n", - "\n", - "In our waypoint case: We add control costs for both: homing (order 0, ensuring to stay close to homing), and velocities (order 1, penalizing movement between waypoints)\n", - "\n", - "And the `addObjective` method now makes use of `times` argument: Specifying [1] means that this objective only holds in the interval [1,1], i.e. at phase-time 1 only." - ] - }, - { - "cell_type": "markdown", - "id": "132f82a0", - "metadata": {}, - "source": [ - "## Path example\n", - "\n", - "Let's do almost the same, but for a fine path. First order=1, leading to zig-zag, then order=2, leading to smooth path." - ] - }, - { - "cell_type": "code", - "execution_count": 19, - "id": "dd21ae9e", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "{ time: 0.013282, evals: 11, done: 1, feasible: 1, sos: 2.51979, f: 0, ineq: 0, eq: 0.00172374 }\n", - "size of path: (40, 7)\n" - ] - } - ], - "source": [ - "# Note, the slicesPerPhase=10 is the only difference to above\n", - "C.setJointState(qHome)\n", - "komo = ry.KOMO(C, 4, 10, 1, False)\n", - "komo.addControlObjective([], 0, 1e-1) # what happens if you change weighting to 1e0? why?\n", - "komo.addControlObjective([], 1, 1e0)\n", - "komo.addObjective([1], ry.FS.positionDiff, ['l_gripper', 'way1'], ry.OT.eq, [1e1])\n", - "komo.addObjective([2], ry.FS.positionDiff, ['l_gripper', 'way2'], ry.OT.eq, [1e1])\n", - "komo.addObjective([3], ry.FS.positionDiff, ['l_gripper', 'way3'], ry.OT.eq, [1e1])\n", - "komo.addObjective([4], ry.FS.positionDiff, ['l_gripper', 'way4'], ry.OT.eq, [1e1])\n", - "\n", - "ret = ry.NLP_Solver(komo.nlp(), verbose=0 ) .solve()\n", - "print(ret)\n", - "q = komo.getPath()\n", - "print('size of path:', q.shape)\n", - "\n", - "for t in range(q.shape[0]):\n", - " C.setJointState(q[t])\n", - " C.view(False, f'waypoint {t}')\n", - " time.sleep(.1)" - ] - }, - { - "cell_type": "code", - "execution_count": 20, - "id": "40341e34", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "{ time: 0.025419, evals: 25, done: 1, feasible: 1, sos: 16.5171, f: 0, ineq: 0, eq: 0.000772768 }\n", - "size of path: (40, 7)\n" - ] - } - ], - "source": [ - "# only differences: the kOrder=2, control objective order 2, constrain final jointState velocity to zero\n", - "C.setJointState(qHome)\n", - "komo = ry.KOMO(C, 4, 10, 2, False)\n", - "komo.addControlObjective([], 0, 1e-1) # what happens if you change weighting to 1e0? why?\n", - "komo.addControlObjective([], 2, 1e0)\n", - "komo.addObjective([1], ry.FS.positionDiff, ['l_gripper', 'way1'], ry.OT.eq, [1e1])\n", - "komo.addObjective([2], ry.FS.positionDiff, ['l_gripper', 'way2'], ry.OT.eq, [1e1])\n", - "komo.addObjective([3], ry.FS.positionDiff, ['l_gripper', 'way3'], ry.OT.eq, [1e1])\n", - "komo.addObjective([4], ry.FS.positionDiff, ['l_gripper', 'way4'], ry.OT.eq, [1e1])\n", - "komo.addObjective([4], ry.FS.jointState, [], ry.OT.eq, [1e1], [], order=1)\n", - "\n", - "ret = ry.NLP_Solver(komo.nlp(), verbose=0 ) .solve()\n", - "print(ret)\n", - "q = komo.getPath()\n", - "print('size of path:', q.shape)\n", - "\n", - "for t in range(q.shape[0]):\n", - " C.setJointState(q[t])\n", - " C.view(False, f'waypoint {t}')\n", - " time.sleep(.1)" - ] - }, - { - "cell_type": "markdown", - "id": "154ea039", - "metadata": {}, - "source": [ - "Notice the new last objective! Without it, *final velocity* would not be zero. The last objective constrains the order=1 (i.e. velocity!) of the jointState feature to be zero.\n", - "\n", - "Let's plot the trajectory:" - ] - }, - { - "cell_type": "code", - "execution_count": 21, - "id": "3d47d887", - "metadata": {}, - "outputs": [ - { - "data": { - "image/png": "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", - "text/plain": [ - "
" - ] - }, - "metadata": {}, - "output_type": "display_data" - } - ], - "source": [ - "import matplotlib.pyplot as plt\n", - "plt.plot(q)\n", - "plt.show()" - ] - }, - { - "cell_type": "code", - "execution_count": 22, - "id": "08aacd1a", - "metadata": {}, - "outputs": [], - "source": [ - "del C" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "c209e80b", - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3 (ipykernel)", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/notebooks/1d-real.ipynb b/notebooks/1d-real.ipynb deleted file mode 100644 index 21b43a6cc..000000000 --- a/notebooks/1d-real.ipynb +++ /dev/null @@ -1,387 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "59a99259", - "metadata": {}, - "source": [ - "# Starting with a real robot\n", - "\n", - "The BotOp interface should work equally for a simulated and real robot. Note that currently only a locally compiled robotic package supports connecting to a Franka robot and Realsense camera. Here some first cautious steps to get started with a real robot." - ] - }, - { - "cell_type": "markdown", - "id": "fee0684d", - "metadata": {}, - "source": [ - "## First robot motion, camera & pcl, and gripper motion" - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "id": "eca938ab", - "metadata": {}, - "outputs": [], - "source": [ - "from robotic import ry\n", - "import numpy as np\n", - "import time\n", - "print(ry.compiled())" - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "id": "950397d7", - "metadata": {}, - "outputs": [], - "source": [ - "#in case you switch to simulation\n", - "ry.params_add({'botsim/verbose': 1., 'physx/motorKp': 10000., 'physx/motorKd': 1000.})\n", - "ry.params_print()" - ] - }, - { - "cell_type": "markdown", - "id": "e3280d7b", - "metadata": {}, - "source": [ - "First, also for a real robot we first load the configuration and maintain our own workspace configuration." - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "id": "33f75e77", - "metadata": {}, - "outputs": [], - "source": [ - "C = ry.Config()\n", - "C.addFile(ry.raiPath('../rai-robotModels/scenarios/pandaSingle.g'))\n", - "pcl = C.addFrame('pcl', 'cameraWrist')\n", - "C.view(False, 'this is your workspace data structure C -- NOT THE SIMULTATION')" - ] - }, - { - "cell_type": "markdown", - "id": "f0a194da", - "metadata": {}, - "source": [ - "Now we start botop with real=True. By passing the model configuration C, the system knows which and how many robots there should be an tries to connect with them." - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "id": "f1119ecc", - "metadata": {}, - "outputs": [], - "source": [ - "# True = real robot!!\n", - "bot = ry.BotOp(C, useRealRobot=True)" - ] - }, - { - "cell_type": "markdown", - "id": "570afef1", - "metadata": {}, - "source": [ - "If that failed, you could `ry.params_print()` to see which global parameters were used and whether you should change them." - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "id": "bafdead7", - "metadata": {}, - "outputs": [], - "source": [ - "if bot.get_t()==0: #if the above failed, use a sim...\n", - " del bot\n", - " bot = ry.BotOp(C, useRealRobot=False)" - ] - }, - { - "cell_type": "markdown", - "id": "b7d17dd0", - "metadata": {}, - "source": [ - "A first **motion**:" - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "id": "df4ea390", - "metadata": {}, - "outputs": [], - "source": [ - "q = bot.get_qHome()\n", - "q[1] = q[1] + .2\n", - "\n", - "bot.moveTo(q)\n", - "bot.wait(C)\n", - "print('first motion done')\n", - "\n", - "bot.moveTo(bot.get_qHome())\n", - "bot.wait(C)" - ] - }, - { - "cell_type": "markdown", - "id": "a845b0fb", - "metadata": {}, - "source": [ - "Grabbing a **camera image & pcl**, adding the pcl to the workspace C:" - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "id": "1b2144a8", - "metadata": {}, - "outputs": [], - "source": [ - "pcl = C.getFrame(\"pcl\")\n", - "pcl.setShape(ry.ST.pointCloud, [2]) # the size here is pixel size for display\n", - "bot.sync(C)\n", - "\n", - "while bot.getKeyPressed()!=ord('q'):\n", - " image, depth, points = bot.getImageDepthPcl(\"cameraWrist\")\n", - " pcl.setPointCloud(points, image)\n", - " pcl.setColor([1,0,0])\n", - " bot.sync(C, .1)" - ] - }, - { - "cell_type": "markdown", - "id": "e0563891", - "metadata": {}, - "source": [ - "Closing & opening the **gripper**:" - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "id": "79a8549c", - "metadata": {}, - "outputs": [], - "source": [ - "#slow close\n", - "bot.gripperMove(ry._left, width=.0, speed=.1)\n", - "\n", - "while not bot.gripperDone(ry._left):\n", - " bot.sync(C)\n", - "\n", - "#fast open\n", - "bot.gripperMove(ry._left, width=.08, speed=1.)\n", - "\n", - "while not bot.gripperDone(ry._left):\n", - " bot.sync(C)" - ] - }, - { - "cell_type": "markdown", - "id": "0f41998c", - "metadata": {}, - "source": [ - "Always shut down the robot properly by destroying the handle:" - ] - }, - { - "cell_type": "code", - "execution_count": 9, - "id": "10443e09", - "metadata": {}, - "outputs": [], - "source": [ - "del bot\n", - "del C" - ] - }, - { - "cell_type": "markdown", - "id": "c1278498", - "metadata": {}, - "source": [ - "## Advanced: Compliance & Force/Torque feedback" - ] - }, - { - "cell_type": "code", - "execution_count": 19, - "id": "6e82d8f8", - "metadata": {}, - "outputs": [], - "source": [ - "from robotic import ry\n", - "import numpy as np\n", - "import time" - ] - }, - { - "cell_type": "code", - "execution_count": 20, - "id": "aa1a93be", - "metadata": {}, - "outputs": [], - "source": [ - "C = ry.Config()\n", - "C.addFile(ry.raiPath('scenarios/pandaSingle.g'))\n", - "C.view(False, 'this is your workspace data structure C -- NOT THE SIMULTATION')" - ] - }, - { - "cell_type": "code", - "execution_count": 21, - "id": "5844a69c", - "metadata": {}, - "outputs": [], - "source": [ - "# True = real robot!!\n", - "bot = ry.BotOp(C, True)" - ] - }, - { - "cell_type": "markdown", - "id": "fba6e622", - "metadata": {}, - "source": [ - "After opening the robot, it is holding its position. Try moving it you can feel the gains." - ] - }, - { - "cell_type": "code", - "execution_count": 22, - "id": "18468cf5", - "metadata": {}, - "outputs": [], - "source": [ - "C.view(True, 'floating=False, damping=True -- Try moving the robot by hand!\\n-- press any key to continue --')" - ] - }, - { - "cell_type": "markdown", - "id": "63ee0298", - "metadata": {}, - "source": [ - "We can let it float (=setting the reference always to q_current) and turn off the damping, which makes the robot move more freely:" - ] - }, - { - "cell_type": "code", - "execution_count": 23, - "id": "953b9f3f", - "metadata": {}, - "outputs": [], - "source": [ - "bot.hold(floating=True, damping=False)\n", - "C.view(True, 'floating=True, damping=False -- Try moving the robot by hand!\\n-- press any key to continue --')" - ] - }, - { - "cell_type": "markdown", - "id": "f79a4d54", - "metadata": {}, - "source": [ - "We can also float with daming:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "5d51abbd", - "metadata": {}, - "outputs": [], - "source": [ - "bot.hold(floating=True, damping=True)\n", - "C.view(True, 'floating=True, damping=True -- Try moving the robot by hand!\\n-- press any key to continue --')" - ] - }, - { - "cell_type": "markdown", - "id": "3e838508", - "metadata": {}, - "source": [ - "The `hold` methods above might be useful for kinestetic teaching or so (you can always keep C sync'ed and compute any features while moving the robot).\n", - "\n", - "But for autonomous compliant manipulation we want to follow a reference and impose compliance in following this reference *along some task space dimensions only*. I.e., a task space compliance." - ] - }, - { - "cell_type": "code", - "execution_count": 16, - "id": "86cad3be", - "metadata": {}, - "outputs": [], - "source": [ - "bot.moveTo(bot.get_qHome(), 1.)\n", - "bot.wait(C)\n", - "\n", - "while True:\n", - " bot.sync(C, .1)\n", - " if bot.getKeyPressed()==ord('q'):\n", - " break\n", - " y, J = C.eval(ry.FS.position, [\"l_gripper\"], [[1,0,0]])\n", - " bot.setCompliance(J, 1.)\n", - " print(' direct:', J @ bot.get_tauExternal(),\n", - " ' pseudoInv:', np.linalg.pinv(J.T, rcond=1e-3) @ bot.get_tauExternal())\n", - "\n", - "bot.setCompliance([], 0.)" - ] - }, - { - "cell_type": "code", - "execution_count": 17, - "id": "549b1e19", - "metadata": {}, - "outputs": [], - "source": [ - "bot.home(C)" - ] - }, - { - "cell_type": "code", - "execution_count": 18, - "id": "248e8726", - "metadata": {}, - "outputs": [], - "source": [ - "del bot\n", - "del C" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "ba70ac7a", - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3 (ipykernel)", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/notebooks/Makefile b/notebooks/Makefile deleted file mode 100644 index 15e47987f..000000000 --- a/notebooks/Makefile +++ /dev/null @@ -1,21 +0,0 @@ - -ipynb_paths = $(shell find . -maxdepth 1 -type f -name '*.ipynb' -not -name '*checkpoint*' -not -name '*nbconvert*' -printf "%f ") - -#run: -# @for p in $(ipynb_paths); do jupyter-nbconvert --to notebook --execute $$p; done - -run: $(ipynb_paths:%=run/%) -clean: $(ipynb_paths:%=clean/%) -convert: $(ipynb_paths:%=convert/%) - -clean/%.ipynb: %.ipynb - +@-jupyter-nbconvert --clear-output --inplace $< - rm -f *.nbconvert.ipynb - -convert/%.ipynb: %.ipynb - +@-jupyter-nbconvert --to python $< - -run/%.ipynb: %.ipynb - +@-echo "=========== run $< =======" - +@-jupyter-nbconvert --to notebook --execute $< - rm -f *.nbconvert.ipynb diff --git a/notebooks/README.md b/notebooks/README.md deleted file mode 100644 index b3eec9657..000000000 --- a/notebooks/README.md +++ /dev/null @@ -1,13 +0,0 @@ -## Tutorials - -Only a few of the tutorials exist yet. Please see the also [docs/](docs/) path. The plan is: - -1. [Basics:](1-basics.ipynb) Configurations, Features & Jacobians -1. [Features:](2-features.ipynb) Learn about the language to define and query features and their Jacobians. Including querying collision features (whether and which objects are in collision). -1. [IK:](3-IK-optimization.ipynb) The simplest use of KOMO is for inverse kinematics - learn how to add features to an optimization problem -1. [KOMO:](4-path-optimization.ipynb) Proper path optimization examples -1. [CGO:](5-cgo-optimization.ipynb) KOMO can also used in "dense" mode, where it optimize as constraint graph -1. [Skeletons:](6-KOMO-skeleton.ipynb) Instead of specifying features low-level, you can specify a skeleton and query a pre-defined bound for that skeleton (path, or sequence). This is kind of a higher level language to set objectives. (But not as general as low-level features. You can mix both.) -1. [Robot Models:](9-robotModels.ipynb) Some info on which scene/robot models are available and how to convert from URDF -1. [LGP:](lgp1-pickAndPlace.ipynb) The first full LGP demo - for now only for pickAndPlace -1. [bullet:](sim1-bullet.ipynb) calling bullet (requires to comment 'BULLET=0' in [config.mk](../config.mk) before compilation) diff --git a/notebooks/botop3-vision.ipynb b/notebooks/botop3-vision.ipynb deleted file mode 100644 index 698e0d9d8..000000000 --- a/notebooks/botop3-vision.ipynb +++ /dev/null @@ -1,267 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# First example to grap images from a webcam\n", - "(but this only gives you an rgb, not depth)\n", - "hit 'q' in the window to stop" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import cv2 as cv" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "cap = cv.VideoCapture(0)\n", - "while(True):\n", - " # Capture frame-by-frame\n", - " ret, frame = cap.read()\n", - " # Our operations on the frame come here\n", - " img = frame\n", - " #img = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)\n", - " # Display the resulting frame\n", - " if len(frame)>0: cv.imshow('img', img)\n", - " if cv.waitKey(1) & 0xFF == ord('q'):\n", - " break" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# When everything done, release the capture\n", - "cap.release()\n", - "cv.destroyAllWindows()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## 2. Example to convert simulated RGB-D images into point clouds" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import os\n", - "os._exit(0)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import cv2 as cv\n", - "import numpy as np\n", - "from robotic import ry\n", - "import time\n", - "print(cv.__version__)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C = ry.Config()\n", - "C.addFile(ry.raiPath('../rai-robotModels/scenarios/pandasTable.g'))\n", - "\n", - "obj = C.addFrame('obj')\n", - "obj.setPose('t(0. 0.1 0.8)')\n", - "obj.setShape(ry.ST.ssBox, size=[.05,.05,.05,.005])\n", - "obj.setColor([1,.0,0])\n", - "obj.setMass(.1)\n", - "obj.setContact(True)\n", - "C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "bot = ry.BotOp(C, False)\n", - "bot.home(C)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "fxypxy = bot.getCameraFxypxy(\"camera\")\n", - "print(fxypxy)\n", - "cameraFrame = C.getFrame(\"camera\")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "points = []\n", - "tau = .01\n", - "\n", - "for t in range(30):\n", - " time.sleep(0.1)\n", - "\n", - " #grab sensor readings from the simulation\n", - " [rgb, depth] = bot.getImageAndDepth('camera') #we don't need images with 100Hz, rendering is slow\n", - " \n", - " if len(rgb)>0: cv.imshow('OPENCV - rgb',\n", - " cv.cvtColor(rgb, cv.COLOR_BGR2RGB))\n", - " if len(depth)>0: cv.imshow('OPENCV - depth', 0.5* depth)\n", - "\n", - " if cv.waitKey(1) & 0xFF == ord('q'):\n", - " break\n", - " \n", - " bot.sync(C)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "cv.destroyAllWindows()\n", - "del bot\n", - "del C" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## 3. example to use multiple camera attached to different robot frames" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import os\n", - "os._exit(0)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "from robotic import ry\n", - "import numpy as np" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C = ry.Config()\n", - "C.addFile(ry.raiPath('../rai-robotModels/scenarios/pandasTable.g'))\n", - "# change the position of the central sensor\n", - "f = C.frame(\"camera\")\n", - "f.setPosition(f.getPosition()+[0,0,.5])\n", - "# add a frame for the additional camera\n", - "f = C.addFrame(\"r_gripperCamera\", \"r_gripper\")\n", - "f.setShape(ry.ST.camera, [.1])\n", - "f.addAttributes({'focalLength':0.895, 'width':640., 'height':360.})" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "bot = ry.BotOp(C, False)\n", - "bot.home(C)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "img, _ = bot.getImageAndDepth('camera')" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "img, _ = bot.getImageAndDepth('r_gripperCamera')" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "del bot\n", - "del C" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3 (ipykernel)", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 4 -} diff --git a/notebooks/botop4-examples.ipynb b/notebooks/botop4-examples.ipynb deleted file mode 100644 index 825f6ee00..000000000 --- a/notebooks/botop4-examples.ipynb +++ /dev/null @@ -1,629 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "0f4af228", - "metadata": {}, - "source": [ - "# Example for grasping in Sim\n", - "* A grasp motion is designed using KOMO, where the 2-waypoint IK problem defines constraints over a sequence of two configurations, one for each waypoint.\n", - "* Each waypoint is here quite rigidly constrained by 6D endeff pose. For convenience, both endeff poses have been defined as 'marker' frames in the configuration. This is a common pattern: Define some convenient reference frames in the configuration, then define NLP constraints relative to these.\n", - "* The solver returns a sequence of two joint positions.\n", - "* Both are added to the BotOp spline pipeline (with hard timings 2 and 3 sec for them). They define a spline transitioning smoothly through both waypoints.\n", - "* The gripper is closed. (In simulation, a hard kinematic link is created!)\n", - "* After homing, the gripper is opened. (In simulation, the hard kinematic link is broken again; the object falls.)" - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "id": "97088a7b", - "metadata": {}, - "outputs": [], - "source": [ - "from robotic import ry\n", - "import numpy as np\n", - "import time" - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "id": "ea930fba", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "-- ry.cpp:operator():101(0) python,\n", - "message: \"Hello, this is been loaded\",\n", - "LGP/cameraFocus: [1, 0.5, 1],\n", - "LGP/collisions: 1,\n", - "LGP/stopTime: 300,\n", - "LGP/stopSol: 6,\n", - "opt/maxStep: 0.1,\n", - "opt/verbose: 6,\n", - "opt/boundedNewton!,\n", - "botsim/engine: physx,\n", - "physx/multiBody!,\n", - "physx/motorKp: 10000,\n", - "physx/motorKd: 1000,\n", - "physx/angularDamping: 10,\n", - "physx/defaultFriction: 100\n" - ] - } - ], - "source": [ - "ry.params_add({'physx/motorKp': 10000., 'physx/motorKd': 1000., 'physx/angularDamping': 10., 'physx/defaultFriction': 100.})\n", - "ry.params_print()" - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "id": "44db23a8", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 3, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C = ry.Config()\n", - "C.addFile(ry.raiPath('../rai-robotModels/scenarios/pandaSingle.g'))\n", - "C.view(False)" - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "id": "efd02145", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 4, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "# .setShape(ry.ST.sphere, size=[.025]) \\\n", - "C.addFrame('box') \\\n", - " .setPosition([-.25,.1,.675]) \\\n", - " .setShape(ry.ST.ssBox, size=[.05,.05,.05,.005]) \\\n", - " .setColor([1,.5,0]) \\\n", - " .setMass(.1) \\\n", - " .setContact(True)\n", - "C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "id": "9588750e", - "metadata": {}, - "outputs": [], - "source": [ - "# WAYPOINT ENGINEERING:\n", - "# manually define frames as an endeff waypoints, relative to box:\n", - "way0 = C.addFrame('way0', 'box')\n", - "way1 = C.addFrame('way1', 'box')" - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "id": "8f6639cc", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "86" - ] - }, - "execution_count": 6, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "way0.setShape(ry.ST.marker, size=[.1])\n", - "way0.setRelativePose('t(0 0 .1) d(90 0 0 1)')\n", - "\n", - "way1.setShape(ry.ST.marker, size=[.1])\n", - "way1.setRelativePose('d(90 0 0 1)')\n", - "\n", - "C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "id": "8b4b9f76", - "metadata": {}, - "outputs": [], - "source": [ - "# define a 2 waypoint problem in KOMO\n", - "komo = ry.KOMO()\n", - "komo.setConfig(C, True)\n", - "komo.setTiming(2., 1, 5., 0)\n", - "komo.addControlObjective([], 0, 1e-0)\n", - "komo.addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.eq);\n", - "komo.addObjective([], ry.FS.jointLimits, [], ry.OT.ineq);\n", - "komo.addObjective([1.], ry.FS.poseDiff, ['l_gripper', 'way0'], ry.OT.eq, [1e1]);\n", - "komo.addObjective([2.], ry.FS.poseDiff, ['l_gripper', 'way1'], ry.OT.eq, [1e1]);" - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "id": "c918d324", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "====nlp==== method:AugmentedLagrangian bounded: no\n", - "==nlp== it:0 evals:0 mu:1 nu:1 muLB:0.1\n", - "----newton---- initial point f(x):93.1593 alpha:1 beta:1\n", - "--newton-- it: 1 |Delta|: 0.2 alpha: 1 evals: 2 f(y): 69.6169 ACCEPT\n", - "--newton-- it: 2 |Delta|: 0.2 alpha: 1 evals: 3 f(y): 47.7566 ACCEPT\n", - "--newton-- it: 3 |Delta|: 0.2 alpha: 1 evals: 4 f(y): 28.1096 ACCEPT\n", - "--newton-- it: 4 |Delta|: 0.2 alpha: 1 evals: 5 f(y): 11.4588 ACCEPT\n", - "--newton-- it: 5 |Delta|: 0.2 alpha: 1 evals: 6 f(y): 6.02281 ACCEPT\n", - "--newton-- it: 6 |Delta|: 0.2 alpha: 1 evals: 7 f(y): 4.0943 ACCEPT\n", - "--newton-- it: 7 |Delta|: 0.170104 alpha: 1 evals: 8 f(y): 3.57494 ACCEPT\n", - "--newton-- it: 8 |Delta|: 0.0579509 alpha: 1 evals: 9 f(y): 3.52862 ACCEPT\n", - "--newton-- it: 9 |Delta|: 0.0238862 alpha: 1 evals: 10 f(y): 3.52229 ACCEPT\n", - "--newton-- stopping: 'absMax(D{ time: 0.016744, evals: 18, done: 1, feasible: 1, sos: 4.37426, f: 0, ineq: 0, eq: 0.000145596 }\n", - "elta)0:\n", - " bot.sync(C, .1)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "f7e3a9d9", - "metadata": {}, - "outputs": [], - "source": [ - "bot.gripperCloseGrasp(ry._left, 'box')\n", - "while not bot.gripperDone(ry._left):\n", - " bot.sync(C, .1)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "ddad80d8", - "metadata": {}, - "outputs": [], - "source": [ - "bot.home(C)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "e219ea7d", - "metadata": {}, - "outputs": [], - "source": [ - "bot.gripperOpen(ry._left)\n", - "while not bot.gripperDone(ry._left):\n", - " bot.sync(C, .1)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "c665847d", - "metadata": {}, - "outputs": [], - "source": [ - "del bot" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "ecc1c120", - "metadata": {}, - "outputs": [], - "source": [ - "del C" - ] - }, - { - "cell_type": "markdown", - "id": "6b6de9d6", - "metadata": {}, - "source": [ - "## Example for pushing in sim\n", - "* A push motion is designed using KOMO, there the 2-waypoint IK problem defines constraints over a sequence of two configurations, one for each waypoint.\n", - "* Each waypoint is here quite rigidly constrained by 6D endeff pose. For convenience, both endeff poses have been defined as 'marker' frames in the configuration. This is a common pattern: Define some convenient reference frames in the configuration, then define NLP constraints relative to these.\n", - "* The solver returns a sequence of two joint positions.\n", - "* Both are added to the BotOp spline pipeline. They define a linear spline interpolation with zero end velocity for both waypoints." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "765fa2ab", - "metadata": {}, - "outputs": [], - "source": [ - "from robotic import ry\n", - "import numpy as np\n", - "import time" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "c896e109", - "metadata": {}, - "outputs": [], - "source": [ - "print('ry version', ry.__version__, ry.compiled())\n", - "ry.params_add({'physx/motorKp': 10000., 'physx/motorKd': 1000.})\n", - "ry.params_print()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "4be73380", - "metadata": {}, - "outputs": [], - "source": [ - "C = ry.Config()\n", - "C.addFile(ry.raiPath('../rai-robotModels/scenarios/pandaSingle.g'))\n", - "C.view(False)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "65f34495", - "metadata": {}, - "outputs": [], - "source": [ - "C.addFrame('box') \\\n", - " .setPosition([-.25,.1,.675]) \\\n", - " .setShape(ry.ST.ssBox, size=[.05,.05,.05,.005]) \\\n", - " .setColor([1,.5,0]) \\\n", - " .setMass(.1) \\\n", - " .setContact(True)\n", - "C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "74fceeef", - "metadata": {}, - "outputs": [], - "source": [ - "# WAYPOINT ENGINEERING:\n", - "# manually define frames as an endeff waypoints, relative to box:\n", - "way0 = C.addFrame('way0', 'box')\n", - "way1 = C.addFrame('way1', 'box')" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "aab75e2e", - "metadata": {}, - "outputs": [], - "source": [ - "way0.setShape(ry.ST.marker, size=[.1])\n", - "way0.setRelativePose('t(-.07 0 0) d(90 0 0 1)')\n", - "\n", - "way1.setShape(ry.ST.marker, size=[.1])\n", - "way1.setRelativePose('t(.2 0 0) d(90 0 0 1)')\n", - "\n", - "C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "64db9873", - "metadata": {}, - "outputs": [], - "source": [ - "# define a 2 waypoint problem in KOMO\n", - "komo = ry.KOMO()\n", - "komo.setConfig(C, True)\n", - "komo.setTiming(2., 1, 5., 0)\n", - "komo.addControlObjective([], 0, 1e-0)\n", - "komo.addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.eq);\n", - "komo.addObjective([], ry.FS.jointLimits, [], ry.OT.ineq);\n", - "komo.addObjective([1.], ry.FS.poseDiff, ['l_gripper', 'way0'], ry.OT.eq, [1e1]);\n", - "komo.addObjective([2.], ry.FS.poseDiff, ['l_gripper', 'way1'], ry.OT.eq, [1e1]);" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "2b0516c0", - "metadata": {}, - "outputs": [], - "source": [ - "ret = ry.NLP_Solver() \\\n", - " .setProblem(komo.nlp()) \\\n", - " .setOptions( stopTolerance=1e-2, verbose=4 ) \\\n", - " .solve()\n", - "print(ret)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "e172b016", - "metadata": {}, - "outputs": [], - "source": [ - "komo.view(False, \"waypoints solution\")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "bd7c9300", - "metadata": {}, - "outputs": [], - "source": [ - "komo.view_close()\n", - "path = komo.getPath()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "8c9e0130", - "metadata": {}, - "outputs": [], - "source": [ - "bot = ry.BotOp(C, False)\n", - "bot.home(C)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "fef0c956", - "metadata": {}, - "outputs": [], - "source": [ - "bot.home(C)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "03b8842d", - "metadata": {}, - "outputs": [], - "source": [ - "bot.gripperClose(ry._left)\n", - "while not bot.gripperDone(ry._left):\n", - " bot.sync(C, .1)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "ef3e902c", - "metadata": {}, - "outputs": [], - "source": [ - "bot.moveTo(path[0])\n", - "bot.moveTo(path[1])\n", - "while bot.getTimeToEnd()>0:\n", - " bot.sync(C, .1)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "380f6540", - "metadata": {}, - "outputs": [], - "source": [ - "bot.home(C)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "510a7f5a", - "metadata": {}, - "outputs": [], - "source": [ - "bot.gripperOpen(ry._left)\n", - "while not bot.gripperDone(ry._left):\n", - " bot.sync(C, .1)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "25ea3e89", - "metadata": {}, - "outputs": [], - "source": [ - "del bot" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "6a93c1de", - "metadata": {}, - "outputs": [], - "source": [ - "del C" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "53a9ece0", - "metadata": {}, - "outputs": [], - "source": [] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "5b96e8e6", - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3 (ipykernel)", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/notebooks/cameraview.ipynb b/notebooks/cameraview.ipynb deleted file mode 100644 index 333889d19..000000000 --- a/notebooks/cameraview.ipynb +++ /dev/null @@ -1,156 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "a7fa063a", - "metadata": {}, - "source": [ - "# CameraView and Rendering [todo]" - ] - }, - { - "cell_type": "markdown", - "id": "ae3d4e90", - "metadata": {}, - "source": [ - "## Rendering configurations\n", - "\n", - "Camera images and depth are usually accessed via a simulation interface (BotOp). But we can compute images and depth also directly for a given configuration, without really creating a (physical) simulation. Here the basic approach: We can also add a frame, attached to the head, which has no shape associated to it, but create a view is associated with that frame\n", - "TODO: Add nvisii support/export!" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "1762bed5", - "metadata": {}, - "outputs": [], - "source": [ - "C.addFrame(name='camera', parent='head_tilt_link', args='Q: focalLength:.3')\n", - "V = C.cameraView()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "9c55a2ee", - "metadata": {}, - "outputs": [], - "source": [ - "Vimg = V.imageViewer()\n", - "Vseg = V.segmentationViewer()\n", - "Vpcl = V.pointCloudViewer()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "11b8948b", - "metadata": {}, - "outputs": [], - "source": [ - "V.addSensor('kinect', 'endeffKinect', 640, 480, 580./480., -1., [.1, 50.] )\n", - "#V.addSensor(name='camera', frameAttached='camera', width=600, height=400)\n", - "V.selectSensor('kinect')\n", - "[image,depth] = V.computeImageAndDepth()\n", - "seg = V.computeSegmentation()\n", - "pcl = V.computePointCloud(depth)\n", - "print('image shape:', image.shape)\n", - "print('depth shape:', depth.shape)\n", - "print('segmentation shape:', seg.shape)\n", - "print('pcl shape:', pcl.shape)" - ] - }, - { - "cell_type": "markdown", - "id": "f3e7d2cb", - "metadata": {}, - "source": [ - "When we move the robot, that view moves with it:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "6468ec12", - "metadata": {}, - "outputs": [], - "source": [ - "C.setJointState([0.5], ['head_pan_joint'])\n", - "C.setJointState([1.], ['head_tilt_joint'])\n", - "V.updateConfig(C)\n", - "[image,depth] = V.computeImageAndDepth()\n", - "pcl = V.computePointCloud(depth)" - ] - }, - { - "cell_type": "markdown", - "id": "859ffb81", - "metadata": {}, - "source": [ - "To close a view (or destroy a handle to a computational module), we reassign it to zero. We can also remove a frame from the configuration." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "cd0c1262", - "metadata": {}, - "outputs": [], - "source": [ - "Vimg = 0\n", - "Vseg = 0\n", - "Vpcl = 0\n", - "V = 0\n", - "C.delFrame('camera')" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "2227bcf5", - "metadata": {}, - "outputs": [], - "source": [ - "C.view_close()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "920159c0", - "metadata": {}, - "outputs": [], - "source": [] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "26a50413", - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3 (ipykernel)", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/notebooks/configurationEditing.ipynb b/notebooks/configurationEditing.ipynb deleted file mode 100644 index 9cd3dc320..000000000 --- a/notebooks/configurationEditing.ipynb +++ /dev/null @@ -1,363 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "89f8bc99", - "metadata": {}, - "source": [ - "# Importing, Editing & Manipulating Configurations\n", - "* This scripts loads a mini scene defined in 'mini.g'.\n", - "* The 'watchFile' command allows you to edit the file in some other editor, while it is being redisplayed whenever you save.\n", - "* 'set/getJointState' shows how to access degrees-of-freedom of the scene\n", - "* 'eval' shows how to access other features, including its Jacobian" - ] - }, - { - "cell_type": "markdown", - "id": "0b373788", - "metadata": {}, - "source": [ - "## Importing robot models\n", - "Let's first have a look at robot models that are already installed with rai. (They are defined in the `rai-robotModels` github repo, which is partly copied into the `.../site-packages/robotic/rai-robotModels` install path.)" - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "id": "c2135256", - "metadata": {}, - "outputs": [], - "source": [ - "from robotic import ry" - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "id": "294c17f2", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "The path where model files are pre-installed:\n", - " /home/mtoussai/.local/lib/python3.8/site-packages/robotic/rai-robotModels/\n" - ] - } - ], - "source": [ - "print('The path where model files are pre-installed:\\n', ry.raiPath(''))\n", - "# you could overwrite this with:\n", - "# ry.setRaiPath('/home/mtoussai/git/rai-robotModels/')" - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "id": "90cefb18", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "-- WARNING:graph.cpp:read:663(-1) nothing to delete with key 'visual'\n", - "-- WARNING:graph.cpp:edit:485(-1) no nodes edited! (from 'joint: { ctrl_H: 1 }')\n" - ] - }, - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 7, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C = ry.Config()\n", - "C.addFile(ry.raiPath('panda/panda.g')) .setPosition([1.,.0,.0]) .setQuaternion([1,0,0,1])\n", - "C.addFile(ry.raiPath('panda/panda.g'), 'r_') .setPosition([1.5,.0,.0]) .setQuaternion([1,0,0,1])\n", - "C.addFile(ry.raiPath('pr2/pr2.g')) .setPosition([-1.,.0,.0])\n", - "C.addFile(ry.raiPath('baxter/baxter.g')) .setPosition([0,.0,1.]) .setQuaternion([1,0,0,1])\n", - "C.addFile(ry.raiPath('robotiq/robotiq.g')) .setParent(C.getFrame('panda_joint7')) .setRelativePosition([0., 0., .15])\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "id": "37c8573c", - "metadata": {}, - "source": [ - "The `addFile` returns the *first* frame defined in the file, which typically is the base frame of the whole robot. Therefore, by setting it's position, you can move the whole loaded robot.\n", - "\n", - "Sometimes you want to add a model twice, but avoid duplicated frame names. With `addFile` you can specify a `prefix` string (here `r_`) which add that prefix to all frame names of the added robot. This is exemplified with the second panda added here.\n", - "\n", - "The Robotiq example also shows that this base frame can be made a child of other frames -- attaching the robotiq to the panda arm. (Here in addition to the existing gripper, which would first have to be removed using `C.delFrame`.)\n", - "\n", - "The following is a simple helper to articulare all dofs in the display (press `q` to interrupt):" - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "id": "58cdcbf3", - "metadata": {}, - "outputs": [ - { - "ename": "KeyboardInterrupt", - "evalue": "", - "output_type": "error", - "traceback": [ - "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[0;31mKeyboardInterrupt\u001b[0m Traceback (most recent call last)", - "Cell \u001b[0;32mIn[8], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m \u001b[43mC\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43manimate\u001b[49m\u001b[43m(\u001b[49m\u001b[43m)\u001b[49m\n", - "\u001b[0;31mKeyboardInterrupt\u001b[0m: " - ] - } - ], - "source": [ - "C.animate()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "a8b99d2e", - "metadata": {}, - "outputs": [], - "source": [ - "del C" - ] - }, - { - "cell_type": "markdown", - "id": "7dd18584", - "metadata": {}, - "source": [ - "## Conversion from URDF\n", - "The python package should install a script `urdf2rai.py` in `.local/bin`. That converts a urdf-file to a .g-file. For instance, place a `ur10.urdf` and the `ur_description` folder into the same folder and run:\n", - "```\n", - "urdf2rai.py ur10.urdf > ur10_org.g\n", - "```\n", - "That should generate a `ur10_org.g` file that can be displayed with `kinEdit ur10_org.g`. Note, by default that **excludes** exporting collision shapes (as I typically replace them, e.g. by the convex hull of the visual shapes). The `-coll 1` option of the script should include also the collision shapes.\n", - "\n", - "The `rai-robotModels` should have more info on conversion from URDF." - ] - }, - { - "cell_type": "markdown", - "id": "569fbbb1", - "metadata": {}, - "source": [ - "## Interactive Editing\n", - "When developing your own model (robot or scene), you could of course just use the python commands `addFrame` etc. But that might be cumbersome and not interactive enough. The .g-file format is fairly easy to edit. To help doing this more interactively, there is the `watchFile` method:\n", - "\n", - "Open the file you want to edit (here `mini.g`) in any editor. At the same time, open it from within python and display it using `watchFile`. The key benefit is that `watchFile` reloads and re-displays the file whenever it is externally modified (it watches the file's inode). That enables interactive editing.\n", - "\n", - "**TODO**: The following is still not crash-robust when readers throw syntax error. kinEdit better?" - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "id": "0c2bd1ab", - "metadata": {}, - "outputs": [], - "source": [ - "from robotic import ry" - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "id": "97a3d5ff", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 2, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C = ry.Config()\n", - "C.addFile('mini.g')\n", - "C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "32f64861", - "metadata": {}, - "outputs": [], - "source": [ - "C.watchFile('mini.g')" - ] - }, - { - "cell_type": "code", - "execution_count": 10, - "id": "d9a162b7", - "metadata": {}, - "outputs": [], - "source": [ - "del C" - ] - }, - { - "cell_type": "markdown", - "id": "a2eaed4e", - "metadata": {}, - "source": [ - "## How to attach frames - faking grasps\n", - "Note, this is not real grasping. Just editing the kinematic tree in your configuration" - ] - }, - { - "cell_type": "code", - "execution_count": 16, - "id": "4e64cc63", - "metadata": {}, - "outputs": [], - "source": [ - "from robotic import ry\n", - "import numpy as np\n", - "import time" - ] - }, - { - "cell_type": "code", - "execution_count": 13, - "id": "023feac6", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 13, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C = ry.Config()\n", - "C.addFile(ry.raiPath('scenarios/pandasTable.g'))\n", - "C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": 14, - "id": "9dfde73a", - "metadata": {}, - "outputs": [], - "source": [ - "C.attach(\"l_gripper\", \"r_gripper\")" - ] - }, - { - "cell_type": "code", - "execution_count": 19, - "id": "b7da474c", - "metadata": {}, - "outputs": [], - "source": [ - "#move a bit around\n", - "\n", - "q = C.getJointState()\n", - "\n", - "for t in range(30):\n", - " q[0] = np.sin(t/10)\n", - " \n", - " C.setJointState(q)\n", - " C.view()\n", - " time.sleep(0.1)" - ] - }, - { - "cell_type": "code", - "execution_count": 20, - "id": "2f8d26f3", - "metadata": {}, - "outputs": [], - "source": [ - "del C" - ] - }, - { - "cell_type": "markdown", - "id": "67072a61", - "metadata": {}, - "source": [ - "## Advanced: YAML and dict representations" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "6fa89f59", - "metadata": {}, - "outputs": [], - "source": [ - "import yaml\n", - "\n", - "with open(ry.raiPath('panda/panda_clean.g'), 'r', encoding='utf-8') as fil:\n", - " model = yaml.safe_load(fil)\n", - "\n", - "print(model)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "1add3739", - "metadata": {}, - "outputs": [], - "source": [ - "del C" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "938a2461", - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3 (ipykernel)", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/notebooks/features.ipynb b/notebooks/features.ipynb deleted file mode 100644 index d9eef38e1..000000000 --- a/notebooks/features.ipynb +++ /dev/null @@ -1,657 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Differentiable Features & Collision Evaluation\n", - "\n", - "The [1st tutorial](1a-configurations.ipynb) introduced the basic concepts of features.\n", - "The [lecture script section 'Kinematics'](../script/script.html#general-concept-of-differentiable-featuresFeatures) introduces this a bit more formally.\n", - "\n", - "This tutorial first gives an overview over available features, and then provides more details specifically on collision features.\n", - "\n", - "Let's first recap how features are directly computed on a configuration - as introduced in the [1st tutorial](1a-configurations.ipynb):" - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "metadata": {}, - "outputs": [], - "source": [ - "from robotic import ry" - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "metadata": {}, - "outputs": [], - "source": [ - "C = ry.Config()\n", - "C.addFile(ry.raiPath('panda/panda.g'))\n", - "q = C.getJointState()" - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "feature value: [2.89890247e-01 1.25455202e-16 8.05237266e-01] \n", - "Jacobian: [[-1.25455202e-16 4.72237266e-01 -1.15172364e-16 -2.32080381e-01\n", - " 0.00000000e+00 4.48170606e-02 0.00000000e+00 0.00000000e+00]\n", - " [ 2.89890247e-01 6.43685653e-17 5.54002326e-01 2.83784184e-16\n", - " 1.63424512e-01 2.26026356e-16 -1.38777878e-17 0.00000000e+00]\n", - " [ 0.00000000e+00 -2.89890247e-01 -1.79370329e-16 5.11220138e-01\n", - " 0.00000000e+00 2.32670220e-01 0.00000000e+00 0.00000000e+00]]\n" - ] - } - ], - "source": [ - "[y,J] = C.eval(ry.FS.position, ['gripper'])\n", - "print('feature value:', y, '\\nJacobian:', J)" - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "(array([ 0.38205142, -0.70710678, 0.59500984]),\n", - " array([[ 7.07106781e-01, 5.95009840e-01, 3.82051424e-01,\n", - " -5.95009840e-01, 3.82051424e-01, -5.95009840e-01,\n", - " -3.82051424e-01, 0.00000000e+00],\n", - " [ 3.82051424e-01, 8.48324576e-17, 7.07106781e-01,\n", - " 2.12081144e-16, -2.94260250e-01, 3.71142002e-16,\n", - " -7.07106781e-01, 0.00000000e+00],\n", - " [ 0.00000000e+00, -3.82051424e-01, 5.95009840e-01,\n", - " 3.82051424e-01, -5.95009840e-01, 3.82051424e-01,\n", - " -5.95009840e-01, 0.00000000e+00]]))" - ] - }, - "execution_count": 4, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "# the x-axis of the given frame in world coordinates\n", - "C.eval(ry.FS.vectorX, ['gripper'])" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The signature of the `eval` method is\n", - "\n", - "* The feature symbol (`FS.` in python; `FS_` in cpp)\n", - "* The set of frames it refers to, given as list of frame names\n", - "* Optionally: A scale, that can also be a matrix to down-project a feature (see below)\n", - "* Optionally: A target, which changes the zero-point of the features (see below)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## List of features\n", - "Here is a list of the most important feature symbols. The rai code implements a few more -- see `help(ry.FS)` for a full list.\n", - "\n", - "| FS | frames | D | description |\n", - "|:---|:---:|:---:|:---|\n", - "| position | [A] | 3 | 3D position of A in world coordinates |\n", - "| positionDiff | [A,B] | 3 | difference of 3D positions of A and B in world coordinates |\n", - "| positionRel | [A,B] | 3 | 3D position of A in B-coordinates |\n", - "| quaternion | [A] | 4 | 4D quaternion of A in world coordinates\\footnote[There is ways to handle the invariance w.r.t.\\ quaternion sign properly.] |\n", - "| quaternionDiff | [A,B] | 4 | ... |\n", - "| quaternionRel | [A,B] | 4 | ... |\n", - "| pose | [A] | 7 | 7D pose of A in world coordinates |\n", - "| poseDiff | [A,B] | 7 | ... |\n", - "| poseRel | [A,B] | 7 | ... |\n", - "| vectorX | [A] | 3 | The x-basis-vector of frame A in world coordinates |\n", - "| vectorXDiff | [A,B] | 3 | The difference of the above for two frames A and B |\n", - "| vectorXRel | [A,B] | 3 | The x-basis-vector of frame A in B-coordinates |\n", - "| vectorY... | | | same as above |\n", - "| scalarProductXX | [A,B] | 1 | The scalar product of the x-basis-vector of frame A with the x-basis-vector of frame B |\n", - "| scalarProduct... | [A,B] | | as above |\n", - "| angularVel | [A] | 3 | The angular velocity of frame A across two configurations (must be order=1!) |\n", - "| accumulatedCollisions | [] | 1 | The sum of collision penetrations; when negative/zero, nothing is colliding |\n", - "| jointLimits | [] | 1 | The sum of joint limit penetrations; when negative/zero, all joint limits are ok |\n", - "| negDistance | [A,B] | 1 | The NEGATIVE distance between convex meshes A and B, positive for penetration |\n", - "| qItself | [] | n | The configuration joint vector |\n", - "| aboveBox | [A,B] | 4 | when all negative, A is above (inside support of) the box B |\n", - "| insideBox | [A,B] | 6 | when all negative, A is inside the box B |" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Position features\n", - "Let's briefly clarify the difference between `position`, `positionDiff`, and `positionRel`:\n", - "\n", - "* `position` is the position of a frame `A` in world coordinates\n", - "* `positionDiff` is the difference (in world coordinates) of position `B` MINUS position `A`\n", - "* `positionRel` is the position of frame `A` in the RELATIVE coordinates of frame `B`\n", - "\n", - "The best example is if `A` is a camera: Assume you would like to frame `B` to be positioned exactly at coordinate (0,0,-.3) in the coordinate frame of `A` -- meaning exactly 30cm centrally in front of the camera -- then the following feature would evaluate the error:" - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "(array([2.89890247e-01, 1.25455202e-16, 1.72237266e-01]),\n", - " array([[ 0.00000000e+00, 4.72237266e-01, -1.15172364e-16,\n", - " -2.32080381e-01, 0.00000000e+00, 4.48170606e-02,\n", - " 0.00000000e+00, 0.00000000e+00],\n", - " [ 0.00000000e+00, 6.43685653e-17, 5.54002326e-01,\n", - " 2.83784184e-16, 1.63424512e-01, 2.26026356e-16,\n", - " -1.38777878e-17, 0.00000000e+00],\n", - " [ 0.00000000e+00, -2.89890247e-01, -1.79370329e-16,\n", - " 5.11220138e-01, 0.00000000e+00, 2.32670220e-01,\n", - " 0.00000000e+00, 0.00000000e+00]]))" - ] - }, - "execution_count": 5, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C.eval(ry.FS.positionRel, ['gripper', 'panda_joint1'], scale=[1], target=[0, 0, .3])" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Concretely, if you minimize the above feature, the `gripper` will look exactly(=centrally) at `panda_joint1` with 30cm distance." - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Scalar product features\n", - "The `scalarProduct` features are also very useful to define relative between frames (e.g. for grasping). For instance, `(FS.scalarProductXX, {'handL', 'handR'}, target=[1])` says that the scalar product of the x-axes (e.g. directions of the index finger) of both hands should equal 1, which means they are aligned. And\n", - "```\n", - "(FS.scalarProductXY, {'handL', 'handR'})\n", - "(FS.scalarProductXZ, {'handL', 'handR'})\n", - "```\n", - "says that the the x-axis of handL should be orthogonal (zero scalar product) to the y- and z-axis of handR. So this also describes aligning both x-axes. However, this formulation is much more robust, as it has good error gradients around the optimum." - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Scale and target transformation\n", - "Let's explain the `scale` and `target` in detail: Formally, specifying a target and scale redefines a feature to become\n", - "$$\n", - " \\phi(x) \\gets \\texttt{scale} \\cdot (\\phi(x) - \\texttt{target})\n", - "$$\n", - "The target needs to be a $D$-dim vector and defines the zero-point of the feature.\n", - "\n", - "The scale can be\n", - "\n", - "* a scalar (just a factor),\n", - "* a $D$-vector (multiplying element-wise to the feature)\n", - "* or a **matrix**.\n", - "\n", - "We can do interesting things when `scale` is a matrix. For instance, if we only want the $xy$-position of a frame returned, we can choose a matrix $S=\\begin{pmatrix}1 & 0 & 0 \\\\ 0 & 1 & 0\\end{pmatrix}$, which multiplies to the 3D feature to return a 2D feature. In code:" - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "(array([2.89890247e-01, 1.25455202e-16]),\n", - " array([[-1.25455202e-16, 4.72237266e-01, -1.15172364e-16,\n", - " -2.32080381e-01, 0.00000000e+00, 4.48170606e-02,\n", - " 0.00000000e+00, 0.00000000e+00],\n", - " [ 2.89890247e-01, 6.43685653e-17, 5.54002326e-01,\n", - " 2.83784184e-16, 1.63424512e-01, 2.26026356e-16,\n", - " -1.38777878e-17, 0.00000000e+00]]))" - ] - }, - "execution_count": 6, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C.eval(ry.FS.position, ['gripper'], [[1,0,0],[0,1,0]])" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "A very useful application is again if we care about a camera looking at a point: If we want frame `B` to appear centrally in the $xy$-plane of frame `A`, we can mimimize the feature:" - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "(array([2.89890247e-01, 1.25455202e-16]),\n", - " array([[ 0.00000000e+00, 4.72237266e-01, -1.15172364e-16,\n", - " -2.32080381e-01, 0.00000000e+00, 4.48170606e-02,\n", - " 0.00000000e+00, 0.00000000e+00],\n", - " [ 0.00000000e+00, 6.43685653e-17, 5.54002326e-01,\n", - " 2.83784184e-16, 1.63424512e-01, 2.26026356e-16,\n", - " -1.38777878e-17, 0.00000000e+00]]))" - ] - }, - "execution_count": 7, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C.eval(ry.FS.positionRel, ['gripper', 'panda_joint1'], [[1,0,0],[0,1,0]])" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Collision features\n", - "\n", - "Let's evaluate the accumulative collision scalar and its Jacobian" - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "(array([0.]), array([[0., 0., 0., 0., 0., 0., 0., 0.]]))" - ] - }, - "execution_count": 8, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "coll = C.feature(ry.FS.accumulatedCollisions, [])\n", - "\n", - "C.computeCollisions() #collisions/proxies are not automatically computed on set...State\n", - "coll.eval(C)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Let's move into collision and redo this" - ] - }, - { - "cell_type": "code", - "execution_count": 9, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "" - ] - }, - "execution_count": 9, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "from robotic import ry\n", - "C = ry.Config()\n", - "C.clear()\n", - "C.addFile(ry.raiPath('panda/panda.g'))\n", - "C.addFile(ry.raiPath('panda/panda.g'), 'r_')\n", - "base_r = C.getFrame('r_panda_base')\n", - "base_r.setPosition([.0, .5, .0])" - ] - }, - { - "cell_type": "code", - "execution_count": 10, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 10, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C.selectJoints(['panda_joint1', 'panda_joint2', 'r_panda_joint1', 'r_panda_joint2'])\n", - "C.setJointState([1.,-.8,-1.,-.8])\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The configuration is now in collision. We can evaluate between some specific shape-pairs that show this:" - ] - }, - { - "cell_type": "code", - "execution_count": 11, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "(array([0.13976684]),\n", - " array([[ 0.2021495 , -0.06471866, 0.00841045, 0.55724234]]))" - ] - }, - "execution_count": 11, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C.eval(ry.FS.negDistance, ['palm', 'r_palm'])" - ] - }, - { - "cell_type": "code", - "execution_count": 12, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "(array([0.08287262]),\n", - " array([[ 0.14218262, 0.43479204, -0.14218262, 0.43479204]]))" - ] - }, - "execution_count": 12, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C.eval(ry.FS.negDistance, ['panda_coll7', 'r_panda_coll7'])" - ] - }, - { - "cell_type": "code", - "execution_count": 13, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "(array([-0.05097286]),\n", - " array([[-0.05482199, 0.48443921, -0.12142734, 0.29574326]]))" - ] - }, - "execution_count": 13, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C.eval(ry.FS.negDistance, ['panda_coll6', 'r_panda_coll6'])" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "However, the above features only return penetration (=negDistance) between specific pairs of shapes. For holistic collision checking we query if any pair of shapes collides. This is called **broad phase collision checking**. The following does this (calling `fcl` internally):" - ] - }, - { - "cell_type": "code", - "execution_count": 14, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "[('r_palm', 'palm', -0.13976683583198263),\n", - " ('r_finger1', 'palm', 0.013576748081966544),\n", - " ('r_finger2', 'palm', 0.014228713904190754),\n", - " ('r_panda_coll6', 'palm', -0.015404140037349565),\n", - " ('r_panda_coll5', 'palm', 0.04525630035427913),\n", - " ('r_finger2', 'r_panda_coll5', 0.13749756426853948),\n", - " ('r_panda_coll7', 'palm', -0.09280264001977123),\n", - " ('r_panda_coll2', 'r_panda_coll5', 0.2045494196968938),\n", - " ('r_panda_coll7', 'r_panda_coll4', 0.32362431478923487),\n", - " ('r_panda_coll3', 'palm', 0.36007247820155464),\n", - " ('r_palm', 'r_panda_coll3', 0.35692722195160376),\n", - " ('panda_coll2', 'panda_coll5', 0.2045494196968937),\n", - " ('panda_coll7', 'panda_coll4', 0.32362431478923515),\n", - " ('r_panda_coll2', 'panda_coll0', 0.29717304075316375),\n", - " ('r_panda_coll0', 'panda_coll1', 0.27280115353885176),\n", - " ('r_panda_coll1', 'panda_coll0', 0.2728011535388518),\n", - " ('r_panda_coll0', 'panda_coll0', 0.19999999999999998),\n", - " ('r_panda_coll0b', 'panda_coll1', 0.22160801784824963),\n", - " ('r_panda_coll0b', 'panda_coll0', 0.13131351929875237),\n", - " ('r_panda_coll0', 'panda_coll2', 0.29717304075316375),\n", - " ('r_panda_coll0b', 'panda_coll2', 0.27389416511745424),\n", - " ('r_panda_coll6', 'panda_coll6', 0.050972864653827),\n", - " ('r_panda_coll6', 'panda_coll7', 0.015315130184475917),\n", - " ('r_panda_coll6', 'finger1', 0.05202028460982694),\n", - " ('r_panda_coll6', 'finger2', 0.03488780676276168),\n", - " ('r_panda_coll5', 'panda_coll6', 0.11164377353037033),\n", - " ('r_panda_coll5', 'panda_coll7', 0.08766336859618301),\n", - " ('r_panda_coll5', 'finger1', 0.12895455840607237),\n", - " ('r_panda_coll5', 'finger2', 0.06147141318949159),\n", - " ('r_panda_coll7', 'panda_coll6', 0.013907106655053708),\n", - " ('r_panda_coll7', 'panda_coll7', -0.08287262318906746),\n", - " ('r_panda_coll7', 'finger1', 0.030647107437714566),\n", - " ('r_panda_coll7', 'finger2', 0.03284520763356755),\n", - " ('r_palm', 'panda_coll6', -0.05737613444861561),\n", - " ('r_finger1', 'panda_coll6', -0.020651551158550177),\n", - " ('r_finger2', 'panda_coll6', 0.112620748583009),\n", - " ('r_palm', 'panda_coll7', -0.1273471133054103),\n", - " ('r_finger1', 'panda_coll7', 0.0011556581521950104),\n", - " ('r_finger2', 'panda_coll7', 0.05638875573522109),\n", - " ('r_palm', 'finger1', -0.02159299051272197),\n", - " ('r_palm', 'finger2', -0.014663617396226986),\n", - " ('r_finger2', 'finger1', 0.03712381290870001),\n", - " ('r_finger2', 'finger2', 0.03836716448878992),\n", - " ('palm', 'panda_coll3', 0.3569272219516041),\n", - " ('r_palm', 'panda_coll3', 0.3884474823677896),\n", - " ('r_panda_coll7', 'panda_coll5', 0.08262699740729021),\n", - " ('r_palm', 'panda_coll5', 0.0151854277723599),\n", - " ('r_finger1', 'panda_coll5', 0.037025559294402516),\n", - " ('r_finger2', 'panda_coll5', 0.16135689397161754)]" - ] - }, - "execution_count": 14, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C.computeCollisions()\n", - "C.getCollisions()" - ] - }, - { - "cell_type": "code", - "execution_count": 15, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0.572477645899696" - ] - }, - "execution_count": 15, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C.getTotalPenetration()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The last command is useful for what is classically called binary collision check: if `totalPenetration` is zero, we have no collisions.\n", - "\n", - "Finally, we can get the same information in a differentiable manner, as a feature:" - ] - }, - { - "cell_type": "code", - "execution_count": 16, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "(array([0.57247765]),\n", - " array([[ 0.5810677 , 1.0999702 , -0.74220228, 1.70678119]]))" - ] - }, - "execution_count": 16, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C.eval(ry.FS.accumulatedCollisions, [])" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Enabling/disabling collisions\n", - "**TODO** explain `setContact` of ry.Frame" - ] - }, - { - "cell_type": "code", - "execution_count": 17, - "metadata": {}, - "outputs": [], - "source": [ - "#those are the real penetrations:\n", - "#C.getCollisions(belowMargin=0.) \n", - "\n", - "#deactivate a collision flag!\n", - "#f = C.getFrame(\"R_gripper\")\n", - "#f.setContact(0)\n", - "\n", - "#C.computeCollisions()\n", - "#C.getCollisions(belowMargin=0.) " - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Notes:\n", - "* The contact flag of frames is an integer: 0 = does not contribute to contact lists, 1 = always contributes, negative = only contributes to collision pairs that are topologically distant in the kinematic tree\n", - "* WARNING: the broad phase collision engine is created at first call - You cannot enable collisions between objects that were disabled during first call of the collision engine\n", - "* TODO: enable recreation of a fresh collision engine for a changed configuration" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Advanced: Features over configuration tuples (path configurations, velocity/acceleration features)\n", - "\n", - "TODO" - ] - }, - { - "cell_type": "code", - "execution_count": 18, - "metadata": {}, - "outputs": [], - "source": [ - "del C" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3 (ipykernel)", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 4 -} diff --git a/notebooks/komo-reporting.ipynb b/notebooks/komo-reporting.ipynb deleted file mode 100644 index 061106c4b..000000000 --- a/notebooks/komo-reporting.ipynb +++ /dev/null @@ -1,510 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "e519d99b", - "metadata": {}, - "source": [ - "# KOMO: Reporting & Explaining Convergence\n", - "\n", - "When designing motion problems using KOMO, it is really important to check feasibility and -- if the point of convergence is infeasible or different to what you expected -- know how to introspect the result. The key here is get information about which constraints were actually active, what their constraint violation is, and \"how much each constraint is pulling\" at the point of convergence. I recently figured a way to quantify the latter (inspecting the gradients the constraints contribute to the underlying Lagrangian), which really \"explains\" the point of convergence as good as it gets.\n", - "\n", - "You should have done the [first KOMO tutorial](1c-komo.ipynb) first." - ] - }, - { - "cell_type": "markdown", - "id": "14de1955", - "metadata": {}, - "source": [ - "## Problem specs reporting\n", - "\n", - "Let's first define a simple feasible KOMO problem (same as in the 1st tutorial), solve it, and get info on convergence:" - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "id": "8e07bf36", - "metadata": {}, - "outputs": [], - "source": [ - "from robotic import ry\n", - "import numpy as np\n", - "import time" - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "id": "059a8ee7", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 2, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C = ry.Config()\n", - "C.addFile(ry.raiPath('scenarios/pandaSingle.g'))\n", - "C.addFrame('box') \\\n", - " .setPosition([-.25,.1,1.]) \\\n", - " .setShape(ry.ST.ssBox, size=[.06,.06,.06,.005]) \\\n", - " .setColor([1,.5,0]) \\\n", - " .setContact(True)\n", - "C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "id": "bccb7b55", - "metadata": {}, - "outputs": [], - "source": [ - "komo = ry.KOMO(C, 1, 1, 0, False) # minimalisitc IK problem: just 1 slice\n", - "komo.addControlObjective([], 0, 1e-1) # basic homing (0th order) objective on joint angles q\n", - "komo.addObjective([], ry.FS.positionDiff, ['l_gripper', 'box'], ry.OT.eq, [1e1]);" - ] - }, - { - "cell_type": "markdown", - "id": "69427e67", - "metadata": {}, - "source": [ - "Even before we run optimization, we can a report containing the komo specs and list of all objectives." - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "id": "97fb23dd", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "{'KOMO_specs': {'x_dim': 7,\n", - " 'dual_dim': 0,\n", - " 'T': 1,\n", - " 'k': 0,\n", - " 'phases': 1.0,\n", - " 'slicesPerPhase': 1,\n", - " 'enableCollisions': False,\n", - " 'tau': 1.0,\n", - " '#slices': 1,\n", - " '#totalDOF': 7,\n", - " '#frames': 65,\n", - " '#pathQueries:': 0},\n", - " 'objectives': {'_F_qItself/0-#14': {'order': 0.0,\n", - " 'type': 'sos',\n", - " 'scale': [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],\n", - " 'times': [],\n", - " 'slices': [0.0, 0.0]},\n", - " '__F_PositionDiff/0-l_gripper-box': {'order': 0.0,\n", - " 'type': 'eq',\n", - " 'scale': [10.0],\n", - " 'times': [],\n", - " 'slices': [0.0, 0.0]}}}" - ] - }, - "execution_count": 4, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "komo.report(False)" - ] - }, - { - "cell_type": "markdown", - "id": "0bdb195d", - "metadata": {}, - "source": [ - "## Constraints error reporting\n", - "\n", - "After optimization, the report will contain errors (i.e. constraint violations, or sqr costs) for all objects. In the current case, all constraint violations are very small:" - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "id": "178e3d42", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "{ time: 0.000383, evals: 6, done: 1, feasible: 1, sos: 0.00414146, f: 0, ineq: 0, eq: 0.00188382 }\n" - ] - } - ], - "source": [ - "ret = ry.NLP_Solver(komo.nlp(), verbose=0 ) .solve()\n", - "q = komo.getPath()\n", - "C.setJointState(q[0])\n", - "C.view()\n", - "print(ret)" - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "id": "d5842764", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "{'_F_qItself/0-#14': {'order': 0.0,\n", - " 'type': 'sos',\n", - " 'scale': [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],\n", - " 'err': 0.004141463188074276,\n", - " 'times': [],\n", - " 'slices': [0.0, 0.0]},\n", - " '__F_PositionDiff/0-l_gripper-box': {'order': 0.0,\n", - " 'type': 'eq',\n", - " 'scale': [10.0],\n", - " 'err': 0.0018838214351309113,\n", - " 'times': [],\n", - " 'slices': [0.0, 0.0]}}" - ] - }, - "execution_count": 6, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "komo.report(False)['objectives']" - ] - }, - { - "cell_type": "markdown", - "id": "96fe745f", - "metadata": {}, - "source": [ - "Let's make the problem infeasible by moving the target out of reach and repeating optimization:" - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "id": "b727e37c", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 7, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C.getFrame('box') .setPosition([.8, .8, 1.])\n", - "C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "id": "5adc15f8", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "{ time: 0.00459, evals: 120, done: 1, feasible: 0, sos: 0.0702195, f: 0, ineq: 0, eq: 4.60313 }\n" - ] - } - ], - "source": [ - "komo.updateRootObjects(C)\n", - "solver = ry.NLP_Solver(komo.nlp(), verbose=0 )\n", - "ret = solver.solve()\n", - "q = komo.getPath()\n", - "C.setJointState(q[0])\n", - "C.view()\n", - "print(ret)" - ] - }, - { - "cell_type": "code", - "execution_count": 9, - "id": "b20fc581", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "{'_F_qItself/0-#14': {'order': 0.0,\n", - " 'type': 'sos',\n", - " 'scale': [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],\n", - " 'err': 0.07021953478317369,\n", - " 'times': [],\n", - " 'slices': [0.0, 0.0]},\n", - " '__F_PositionDiff/0-l_gripper-box': {'order': 0.0,\n", - " 'type': 'eq',\n", - " 'scale': [10.0],\n", - " 'err': 4.603126581420996,\n", - " 'times': [],\n", - " 'slices': [0.0, 0.0]}}" - ] - }, - "execution_count": 9, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "komo.report(False)['objectives']" - ] - }, - { - "cell_type": "markdown", - "id": "46bf2e2b", - "metadata": {}, - "source": [ - "We clearly see that the `positionDiff` eq-objective is violated (`err` includes the `scale` as factor, so err=4.6 means 0.46 meters error)." - ] - }, - { - "cell_type": "markdown", - "id": "a371b003", - "metadata": {}, - "source": [ - "## Lagrange gradients reporting\n", - "\n", - "Seeing only objective errors sometimes does not really explain what is the issues. So instead we can let the solver report what the lagrange gradients w.r.t. each feature are:" - ] - }, - { - "cell_type": "code", - "execution_count": 10, - "id": "ee59d5f3", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "{'__F_PositionDiff/0-l_gripper-box': {'err': 4.603126581420996,\n", - " 'grad': 1996.3797729703783,\n", - " 'type': 'eq'},\n", - " '_F_qItself/0-#14': {'err': 0.07021953478317369,\n", - " 'grad': 0.10441937164903314,\n", - " 'type': 'sos'}}" - ] - }, - "execution_count": 10, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "solver.reportLagrangeGradients(komo.getFeatureNames())" - ] - }, - { - "cell_type": "markdown", - "id": "51c2bca7", - "metadata": {}, - "source": [ - "Note that this list is always sorted, starting with largest gradients first. For complex problems, this report can be rather insightful. Sometimes one explicitly sees which objectives *fight* against each other in the Lagrangian by both having similar large gradient sizes (typically with opposing directions, which the report does not show; also a matrix with \"angles between gradients\" can be computed)." - ] - }, - { - "cell_type": "markdown", - "id": "5d67e74c", - "metadata": {}, - "source": [ - "## Plotting constraint errors over time for paths\n", - "\n", - "The above only considers a simple IK problem. Let's look at a path problem (we take the same 4-waypoint problem from the 1st tutorial):" - ] - }, - { - "cell_type": "code", - "execution_count": 37, - "id": "a6da9bda", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "0" - ] - }, - "execution_count": 37, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "C = ry.Config()\n", - "C.addFile(ry.raiPath('scenarios/pandaSingle.g'))\n", - "C.addFrame('way1'). setShape(ry.ST.marker, [.1]) .setPosition([.4, .2, 1.])\n", - "C.addFrame('way2'). setShape(ry.ST.marker, [.1]) .setPosition([.4, .2, 1.4])\n", - "C.addFrame('way3'). setShape(ry.ST.marker, [.1]) .setPosition([-.4, .2, 1.])\n", - "C.addFrame('way4'). setShape(ry.ST.marker, [.1]) .setPosition([-.4, .2, 1.4])\n", - "q0 = C.getJointState()\n", - "C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": 41, - "id": "40341e34", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "{ time: 0.030868, evals: 25, done: 1, feasible: 1, sos: 16.5172, f: 0, ineq: 0, eq: 0.000774803 }\n" - ] - } - ], - "source": [ - "C.setJointState(q0)\n", - "komo = ry.KOMO(C, 4, 10, 2, False)\n", - "komo.addControlObjective([], 0, 1e-1)\n", - "komo.addControlObjective([], 2, 1e0)\n", - "komo.addObjective([1], ry.FS.positionDiff, ['l_gripper', 'way1'], ry.OT.eq, [1e1])\n", - "komo.addObjective([2], ry.FS.positionDiff, ['l_gripper', 'way2'], ry.OT.eq, [1e1])\n", - "komo.addObjective([3], ry.FS.positionDiff, ['l_gripper', 'way3'], ry.OT.eq, [1e1])\n", - "komo.addObjective([4], ry.FS.positionDiff, ['l_gripper', 'way4'], ry.OT.eq, [1e1])\n", - "komo.addObjective([4], ry.FS.jointState, [], ry.OT.eq, [1e1], [], order=1)\n", - "\n", - "solver = ry.NLP_Solver(komo.nlp(), verbose=0 )\n", - "ret = solver.solve()\n", - "print(ret)\n", - "q = komo.getPath()\n", - "\n", - "for t in range(q.shape[0]):\n", - " C.setJointState(q[t])\n", - " C.view(False, f'waypoint {t}')\n", - " time.sleep(.1)" - ] - }, - { - "cell_type": "markdown", - "id": "2edd1e16", - "metadata": {}, - "source": [ - "Using `plotOverTime` in the report method now plots the constraint violations (or sqr costs) over *phase*, using gnuplot. (You need to have `gnuplot` installed.)\n", - "\n", - "Here, the only interesting (non-zero) signal is from the 2nd-order control costs, which are the acceleration costs. This is a typical acceperation cost profile for optimal paths between waypoints." - ] - }, - { - "cell_type": "code", - "execution_count": 42, - "id": "3f9776fe", - "metadata": {}, - "outputs": [], - "source": [ - "R = komo.report(plotOverTime=True)" - ] - }, - { - "cell_type": "markdown", - "id": "4b96a7c9", - "metadata": {}, - "source": [ - "Finally, the largrange gradients show that the control costs and the final velocity constraints contribute a lot, but each waypoint also contributes a significant gradient that explains the point of convergence. (If you'd increase control cost scaling, the waypoint Lagrange parameters and their reported gradients would equally increase.)" - ] - }, - { - "cell_type": "code", - "execution_count": 43, - "id": "c6b64321", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "{'_F_qItself/2-#14': {'err': 16.271121116334356,\n", - " 'grad': 6007.955825681214,\n", - " 'type': 'sos'},\n", - " '_F_qItself/1-#14': {'err': 3.3030160848102064e-05,\n", - " 'grad': 215.45148937551147,\n", - " 'type': 'eq'},\n", - " '__F_PositionDiff/0-l_gripper-way3': {'err': 0.00010143823277963815,\n", - " 'grad': 38.290278666650735,\n", - " 'type': 'eq'},\n", - " '__F_PositionDiff/0-l_gripper-way1': {'err': 0.00022678390968228213,\n", - " 'grad': 36.94214120773105,\n", - " 'type': 'eq'},\n", - " '__F_PositionDiff/0-l_gripper-way2': {'err': 0.00016656504619849688,\n", - " 'grad': 31.04187318969391,\n", - " 'type': 'eq'},\n", - " '__F_PositionDiff/0-l_gripper-way4': {'err': 0.0002469860763765208,\n", - " 'grad': 22.003650346207117,\n", - " 'type': 'eq'},\n", - " '_F_qItself/0-#14': {'err': 0.2460856623621121,\n", - " 'grad': 1.2108203887336064,\n", - " 'type': 'sos'}}" - ] - }, - "execution_count": 43, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "solver.reportLagrangeGradients(komo.getFeatureNames())" - ] - }, - { - "cell_type": "markdown", - "id": "7ee8271b", - "metadata": {}, - "source": [ - "Test the following: Change the timing of the 3rd waypoint to \"[2]\" as well, requiring the robot to be at way2 and way3 at the same time -- which clearly is infeasible. Check the solver return and lagrange gradients." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "7d08b232", - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3 (ipykernel)", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/notebooks/komo3-switches-skeletons.ipynb b/notebooks/komo3-switches-skeletons.ipynb deleted file mode 100644 index e9fa0e401..000000000 --- a/notebooks/komo3-switches-skeletons.ipynb +++ /dev/null @@ -1,314 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Advanced: Switches & Skeletons" - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "metadata": {}, - "outputs": [], - "source": [ - "from robotic import ry\n", - "import numpy as np" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C = ry.Config()\n", - "\n", - "C.addFile(\"../KOMO/switches/model2.g\")\n", - "C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "metadata": {}, - "outputs": [], - "source": [ - "komo = ry.KOMO()\n", - "\n", - "komo.setModel(C, False)\n", - "komo.setTiming(2.5, 30, 5., 2)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo.add_qControlObjective([], 2)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo.addQuaternionNorms([], 3.)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "#grasp\n", - "komo.addModeSwitch([1., 2.], ry.SY.stable, [\"gripper\", \"box\"])\n", - "komo.addObjective([1.], ry.FS.positionDiff, [\"gripper\", \"box\"], ry.OT.eq, [1e2])\n", - "komo.addObjective([1.], ry.FS.scalarProductXX, [\"gripper\", \"box\"], ry.OT.eq, [1e2], [0.])\n", - "komo.addObjective([1.], ry.FS.vectorZ, [\"gripper\"], ry.OT.eq, [1e2], [0., 0., 1.])\n", - "\n", - "#slow - down - up\n", - "komo.addObjective([1.], ry.FS.qItself, [], ry.OT.eq, [], [], 1)\n", - "komo.addObjective([.9,1.1], ry.FS.position, [\"gripper\"], ry.OT.eq, [], [0.,0.,.1], 2)\n", - "\n", - "#place\n", - "komo.addModeSwitch([2., -1.], ry.SY.stable, [\"table\", \"box\"])\n", - "komo.addObjective([2.], ry.FS.positionDiff, [\"box\", \"table\"], ry.OT.eq, [1e2], [0,0,.08])\n", - "komo.addObjective([2.], ry.FS.vectorZ, [\"gripper\"], ry.OT.eq, [1e2], [0., 0., 1.])\n", - "\n", - "#slow - down - up\n", - "komo.addObjective([2.], ry.FS.qItself, [], ry.OT.eq, [], [], 1)\n", - "komo.addObjective([1.9,2.2], ry.FS.position, [\"gripper\"], ry.OT.eq, [], [0.,0.,.1], 2)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "#komo.verbose = 4;\n", - "komo.optimize();\n", - "#/ komo.checkGradients();\n", - "\n", - "komo.view(False, \"result\")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo.view_play(False, .1)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Same again, but only the keyframes:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo=0" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo = ry.KOMO()\n", - "komo.setModel(C, False)\n", - "komo.setTiming(3., 1, 5., 1) #DIFFERENT" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo.add_qControlObjective([], 1, 1e-1) #DIFFERENT\n", - "komo.addQuaternionNorms([], 3.)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "#grasp\n", - "komo.addModeSwitch([1., 2.], ry.SY.stable, [\"gripper\", \"box\"])\n", - "komo.addObjective([1.], ry.FS.positionDiff, [\"gripper\", \"box\"], ry.OT.eq, [1e2])\n", - "komo.addObjective([1.], ry.FS.scalarProductXX, [\"gripper\", \"box\"], ry.OT.eq, [1e2], [0.])\n", - "komo.addObjective([1.], ry.FS.vectorZ, [\"gripper\"], ry.OT.eq, [1e2], [0., 0., 1.])\n", - "\n", - "#DIFFERENT: UP-DOWN MISSING\n", - " \n", - "#place\n", - "komo.addModeSwitch([2., -1.], ry.SY.stable, [\"table\", \"box\"])\n", - "komo.addObjective([2.], ry.FS.positionDiff, [\"box\", \"table\"], ry.OT.eq, [1e2], [0,0,.08])\n", - "komo.addObjective([2.], ry.FS.vectorZ, [\"gripper\"], ry.OT.eq, [1e2], [0., 0., 1.])" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo.optimize();\n", - "komo.view(False, \"result\")\n", - "print(\"constraints:\", komo.getConstraintViolations())\n", - "print(\"costs:\", komo.getCosts())\n", - "komo.getReport()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo.view_play(False, .2)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Skeletons" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import sys\n", - "sys.path += ['../../lib', '../../build', '../../../build']\n", - "import numpy as np\n", - "from robotic import ry" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C = ry.Config()\n", - "C.addFile(\"../KOMO/skeleton/model.g\")\n", - "C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "S = [\n", - " [1.], ry.SY.topBoxGrasp, [\"gripper\", \"box2\"],\n", - " [1., 2.], ry.SY.stable, [\"gripper\", \"box2\"],\n", - " [1.9,2.1], ry.SY.downUp, [\"gripper\"],\n", - " [2.], ry.SY.poseEq, [\"box2\", \"target2\"],\n", - " [2., 2.2], ry.SY.stable, [\"table\", \"box2\"],\n", - "]" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo = ry.KOMO()\n", - "komo.setModel(C, False)\n", - "komo.setSkeleton(S, ry.arg.path)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "print(komo.reportProblem())" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "#komo.verbose = 4;\n", - "komo.optimize();\n", - "#/ komo.checkGradients();\n", - "\n", - "komo.view(False, \"result\")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo.view_play(False, .1)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo.getReport()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3 (ipykernel)", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 4 -} diff --git a/notebooks/mini.g b/notebooks/mini.g deleted file mode 100644 index dd91dfd07..000000000 --- a/notebooks/mini.g +++ /dev/null @@ -1,3 +0,0 @@ -A: { X: "t(1 0 1) d(30 1 0 0)", shape: marker, size: [.5] } -B(A): { joint:hingeX, q: 0.5, limits: [-2., 2], shape: marker, size: [.3] } -C(B): { Q: "t(0 0 .5) d(30 1 0 0)", shape: marker, size: [.3], color:[1 0 0] } diff --git a/notebooks/opt1-basics.ipynb b/notebooks/opt1-basics.ipynb deleted file mode 100644 index b51c93321..000000000 --- a/notebooks/opt1-basics.ipynb +++ /dev/null @@ -1,139 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Optimization (NLP formulation and solving)\n", - "\n", - "This is completely independent from all robotics code. Provides a generic interface to NLP formulation and calling various solvers." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import sys\n", - "sys.path += ['../../lib', '../../build', '../../../build']\n", - "import numpy as np\n", - "from robotic import ry" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Define a function to compute differentiable features" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "#the function needs to have the signature (array) -> (array, array) with dimensionalities (n) -> (d, d-times-n)\n", - "def sqrPot(x):\n", - " y = np.array(x)\n", - " y[0] = y[0] - 1.\n", - " J = np.eye(y.size)\n", - " return y,J" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Define a NLP (non-linear mathematical program)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "nlp = ry.NLP_Factory()\n", - "nlp.setDimension(3)\n", - "nlp.setBounds([-2,-2,-2],[2,2,2])\n", - "nlp.setFeatureTypes([ry.OT.sos, ry.OT.sos, ry.OT.sos])\n", - "nlp.setEvalCallback(sqrPot)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Define a solver" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "solver = ry.NLP_Solver()\n", - "solver.setProblem(nlp)\n", - "solver.setSolver(ry.NLP_SolverID.newton)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "solver.solve(True)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "solver.getLog_x()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "solver.getLog_costs()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3 (ipykernel)", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 4 -} diff --git a/notebooks/rai.cfg b/notebooks/rai.cfg deleted file mode 100644 index 3ae6b62ad..000000000 --- a/notebooks/rai.cfg +++ /dev/null @@ -1,4 +0,0 @@ -message: "Hello, the local 'rai.cfg' was loaded" - -#botsim/engine: physx #kinematic -#physx/multiBody: false #true diff --git a/notebooks/retired/1-basics.ipynb b/notebooks/retired/1-basics.ipynb deleted file mode 100644 index 36f7530e8..000000000 --- a/notebooks/retired/1-basics.ipynb +++ /dev/null @@ -1,562 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Tutorial 1\n", - "\n", - "more detailed docs: https://marctoussaint.github.io/robotics-course/" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import sys\n", - "sys.path.append('../build')\n", - "import numpy as np\n", - "import libry as ry" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Setting up a basic Config\n", - "\n", - "The starting point is to create a `Configuration`." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C = ry.Config()\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "This shows an empty configuration. Tip: Make the view window appear \"Always On Top\" (right click on the window bar)\n", - "\n", - "You can add things (objects, scene models, robots) to a configuration." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C.clear()\n", - "C.addFile('../rai-robotModels/pr2/pr2.g')\n", - "C.addFile('../rai-robotModels/objects/kitchen.g')\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "You need to call C.view() to update the view" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "ball = C.addFrame(name=\"ball\")\n", - "ball.setShape(ry.ST.sphere, [.1])\n", - "ball.setPosition([.8,.8,1.5])\n", - "ball.setColor([1,1,0])\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "One can also add convex meshes (just passing the vertex array), or use sphere-swept convex meshes (ssBox, capsule, sphere, etc)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C.addFrame(name=\"hand\", parent=\"pr2L\") \\\n", - " .setShape(ry.ST.ssBox, size=[.2,.2,.1,.02]) \\\n", - " .setRelativePosition([0,0,-.1]) \\\n", - " .setColor([1,1,0])\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "In this last example, the new object has another frame (pr2L) as *parent*. This means that it is permanently attached to this parent. pos and quat/rot are interpreted relative to the parent." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "f = C.frame(\"hand\")\n", - "print(\"position:\", f.getPosition())\n", - "print(\"orientation:\", f.getQuaternion())" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Joint and Frame State\n", - "\n", - "A configuration is a tree of n frames. Every frame has a pose (position & quaternion), which is represented as a 7D vector (x,y,z, qw,qx,qy,qz). The frame state is the $n\\times 7$ matrix, where the i-th row is the pose of the i-th frame.\n", - "\n", - "A configuration also defines joints, which means that the relative transfromation from a parent to a child frame is parameterized by degrees-of-freedoms (DOFs). If the configuration has in total n DOFs, the joint state is a n-dimensional vector.\n", - "\n", - "Setting the joint state implies computing all relative transformations, and then forward chaining all transformations to compute all frame poses. So setting the joint state also sets the frame state.\n", - " \n", - "Setting the frame state allows you to set frame poses that are inconsistent/impossible w.r.t. the joints! Setting the frame state implies computing all relative transformations from the frame poses, and then assigning the joint state to the *projection* onto the actual DOFs" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "q = C.getJointState()\n", - "print('joint names: ', C.getJointNames() )\n", - "print('joint state: ', q)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Let's move the configuration by adding to the joint configuration" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "q[2] = q[2] + 1.\n", - "C.setJointState(q)\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The *frame state* is a $n\\times 7$ matrix, which contains for all of $n$ frames the 7D pose. A pose is stored as [p_x, p_y, p_z, q_w, q_x, q_y, q_z], with position p and quaternion q." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "X0 = C.getFrameState()\n", - "print('frame state: ', X0)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Let's do a questionable thing: adding .1 to all numbers in the frame matrix!" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "X = X0 + .1\n", - "C.setFrameState(X)\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The rows of X have non-normalized quaternions! These are normalized when setting the frame state.\n", - "\n", - "Also, the frame poses are now *inconsistent* to the joint constraints! We can read out the projected joint state, set the joint state, and get a consistent state again:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C.setJointState( C.getJointState() )\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Now all *joint* transformations are consistent: just hingeX transformations or alike. However, all the other relative transformations of links and shapes are still messed up from setting their frame pose. Let's bring the configuration back into the state before the harsh *setFrame*" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C.setFrameState(X0)\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - " ## Selecting joints\n", - "\n", - "Often one would like to choose which joints are actually active, that is, which joints are referred to in q. This allows one to sub-select joints and work only with projections of the full configuration state. This changes the joint state dimensionality, including ordering of entries in q.\n", - "\n", - "The frame state is not affected by such a selection of active joints." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C.selectJointsByTag([\"armL\",\"base\"])\n", - "q = C.getJointState()\n", - "print('joint state: ', q)\n", - "print('joint names: ', C.getJointNames() )" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Features & Jacobians\n", - "\n", - "A core part of rai defines features over configurations. A feature is a differentiable mapping from a configuration (or set of configurations) to a vector. Starndard features are \"position-of-endeffector-X\" or \"distance/penetration-between-convex-shapes-A-and-B\", etc. But there are many, many more features defined in rai, like error of Newton-Euler-equations for an object, total energy of the system, etc. Defining differentiable features is the core of many functionalities in the rai code.\n", - "\n", - "Let's define a basic feature over C: the 3D (world coordinate) position of pr2L (left hand)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "F = C.feature(ry.FS.position, [\"pr2L\"])" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "We can now evaluate the feature, and also get the Jacobian:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "print(F.description(C))\n", - "\n", - "[y,J] = F.eval(C)\n", - "print('hand position:', y)\n", - "print('Jacobian:', J)\n", - "print('Jacobian shape:', J.shape)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "We can linearly transform features by setting 'scale' and 'target':" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "F.setScale([10.]) #note: needs to be an array!\n", - "F.setTarget([0., 0., 1.])\n", - "F.eval(C)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Setting scale and target actually transforms the feature to become\n", - "$$\n", - " \\phi(x) \\gets \\texttt{scale} \\cdot (\\phi(x) - \\texttt{target})\n", - "$$\n", - "The $\\cdot$ is flexibly interpreted: scale can be an arbitrary matrix, or a vector of same size as $\\phi$ (element-wise re-scaling), or a single scalar (as above). E.g., if we only care about the height of the robot hand, we can do this:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "F.setScale([[0,0,10]]) #pick z-coordinate only, and scale by 10\n", - "F.setTarget([0., 0., 1.]) #note that the target needs to be in UNSCALED space!\n", - "F.eval(C)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "A negative-distance (penetration) feature example" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "F2 = C.feature(ry.FS.distance, [\"hand\", \"ball\"]) #distance actually means neg-distance\n", - "print(F2.description(C))" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "F2.eval(C)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "When a feature is of higher *order*, by default it computes the difference, acceleration, etc, w.r.t. multiple configurations. We need to create a compound configurations (\"tuple\") of several configurations to evaluate this:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C2 = ry.Config()\n", - "C2.addConfigurationCopy(C,tau=1.) #this replicates the whole structure, but with semantics 'tau'-translated in time\n", - "C2.addConfigurationCopy(C,tau=1.) #this replicates the whole structure\n", - "C2.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "F.setScale([]) #reset to default (no scaling)\n", - "F.setTarget([]) #reset to default (zero target)\n", - "F.setOrder(1)\n", - "C2.getJointState() #[needed here to ensure DOFs are indexed..]\n", - "F.eval(C2)[0]" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "This should be zero, as the difference between the first and second copy is zero. To see a difference, let's move the 2nd configuration:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "q = C.getJointState()\n", - "q = q - .1\n", - "C2.setJointStateSlice(q,1)\n", - "C2.view()\n", - "y = F.eval(C2)[0]\n", - "print('hand difference (y(C2) - y(K)) =', y)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "An acceleration example:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C2.addConfigurationCopy(C)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C2.setJointStateSlice(q + .3, 2)\n", - "C2.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "F.setOrder(2)\n", - "(y,J) = F.eval(C2)\n", - "print('hand acceleration:', y)\n", - "print('shape of Jacobian:', J.shape)\n", - "print(F.description(C2))" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Note that the Jacobian is now w.r.t. all three configurations! It is of size 3x(3xdim(q)). Let's retrieve the Jacobian w.r.t. C3 only:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "J = J.reshape((3,3,q.size))\n", - "print('shape of Jacobian:', J.shape)\n", - "J[:,1,:]" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C=0\n", - "C2=0" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Editing Configurations" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C = ry.Config()\n", - "C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import subprocess\n", - "subprocess.Popen(['gedit', 'tmp.g'])" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "## uncomment this! (Automatic testing for this interactive method does not work...)\n", - "#C.edit('tmp.g')" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C=0" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.6.9" - } - }, - "nbformat": 4, - "nbformat_minor": 2 -} diff --git a/notebooks/retired/2-features.ipynb b/notebooks/retired/2-features.ipynb deleted file mode 100644 index 05211f00f..000000000 --- a/notebooks/retired/2-features.ipynb +++ /dev/null @@ -1,287 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Features and Objectives\n", - "\n", - "This doc is mostly text, explaining the general concept of features, listing the ones defined in rai, and explaining how they define objectives for optimization.\n", - "\n", - "At the bottom there are also examples on the collision features." - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Features\n", - "\n", - "We assume a single configuration $x$, or a whole set of configurations\n", - "$\\{x_1,..,x_T\\}$. Each $x_i \\in\\mathbb{R}$ are the DOFs of that\n", - "configuration.\n", - "\n", - "A feature $\\phi$ is a differentiable mapping\n", - "$$\\phi: x \\mapsto \\mathbb{R}^D$$\n", - "of a single configuration into some $D$-dimensional space, or a mapping\n", - "$$\\phi: (x_0,x_2,..,x_k) \\mapsto \\mathbb{R}^D$$\n", - "of a $(k+1)$-tuple of configurations to a $D$-dimensional space.\n", - "\n", - "The rai code implements many features, most of them are accessible via\n", - "a feature symbol (FS). They are declared in\n", - "https://github.com/MarcToussaint/rai/blob/master/rai/Kin/featureSymbols.h\n", - "\n", - "Here is a table of feature symbols, with the\n", - "respective dimensionality $D$, the default order $k$, and a\n", - "description\n", - "\n", - "| FS | frames | $D$ | $k$ | description |\n", - "|:---:|:---:|:---:|:---:|:---:|\n", - "| position | {o1} | 3 || 3D position of o1 in world coordinates |\n", - "| positionDiff | {o1,o2} | 3 || difference of 3D positions of o1 and o2 in world coordinates |\n", - "| positionRel | {o1,o2} | 3 || 3D position of o1 in o2 coordinates |\n", - "| quaternion | {o1} | 4 || 4D quaternion of o1 in world coordinates\\footnote{There is ways to handle the invariance w.r.t.\\ quaternion sign properly.} |\n", - "| quaternionDiff | {o1,o2} | 4 || ... |\n", - "| quaternionRel | {o1,o2} | 4 || ... |\n", - "| pose | {o1} | 7 || 7D pose of o1 in world coordinates |\n", - "| poseDiff | {o1,o2} | 7 || ... |\n", - "| poseRel | {o1,o2} | 7 || ... |\n", - "| vectorX | {o1} | 3 || The x-axis of frame o1 rotated back to world coordinates |\n", - "| vectorXDiff | {o1,o2} | 3 || The difference of the above for two frames o1 and o2 |\n", - "| vectorXRel | {o1,o2} | 3 || The x-axis of frame o1 rotated as to be seend from the frame o2 |\n", - "| vectorY... | | | | same as above |\n", - "| scalarProductXX | {o1,o2} | 1 || The scalar product of the x-axis fo frame o1 with the x-axis of frame o2 |\n", - "| scalarProduct... | {o1,o2} | | | as above |\n", - "| gazeAt | {o1,o2} | 2 | | The 2D projection of the origin of frame o2 onto the xy-plane of frame o1 |\n", - "| angularVel | {o1} | 3 | 1 | The angular velocity of frame o1 across two configurations |\n", - "| accumulatedCollisions | {} | 1 | | The sum of collision penetrations; when negative/zero, nothing is colliding |\n", - "| jointLimits | {} | 1 | | The sum of joint limit penetrations; when negative/zero, all joint limits are ok |\n", - "| distance | {o1,o1} | 1 | | The NEGATIVE distance between convex meshes o1 and o2, positive for penetration |\n", - "| qItself | {} | $n$ | | The configuration joint vector |\n", - "| aboveBox | {o1,o2} | 4 | | when all negative, o1 is above (inside support of) the box o2 |\n", - "| insideBox | {o1,o2} | 6 | | when all negative, o1 is inside the box o2 |\n", - "| standingAbove | | | | ? |\n", - "\n", - "A features is typically defined by\n", - "* The feature symbol (`FS_...` in cpp; `FS....` in python)\n", - "* The set of frames it refers to\n", - "* Optionally: A target, which changes the zero-point of the features (optimization typically try to drive features to zero, see below)\n", - "* Optionally: A scaling, that can also be a matrix to down-project a feature\n", - "* Optionally: The order $k$, which can make the feature a velocity or acceleration feature\n", - "\n", - "Target and scale redefine a feature to become\n", - "$$\n", - " \\phi(x) \\gets \\texttt{scale} \\cdot (\\phi(x) - \\texttt{target})\n", - "$$\n", - "The target needs to be a $D$-dim vector. The scale can be a matrix, which projects features; e.g., and 3D position to just $x$-position.\n", - "\n", - "The order of a feature is usually $k=0$, meaning that it is defined over a single configuration only. $k=1$ means that it is defined over two configurations (1st oder Markov), and redefines the feature to become the difference or velocity\n", - "$$\n", - " \\phi(x_1,x_2) \\gets \\frac{1}{\\tau}(\\phi(x_2) - \\phi(x_1))\n", - "$$\n", - "$k=2$ means that it is defined over three configurations (2nd order Markov), and redefines the feature to become the acceleration\n", - "$$\n", - " \\phi(x_1,x_2,x_3) \\equiv \\frac{1}{\\tau^2}(\\phi(x_1) + \\phi(x_3) - 2 \\phi(x_2))\n", - "$$" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Examples\n", - "\n", - "```\n", - "(FS.position, {'hand'})\n", - "```\n", - "is the 3D position of the hand in world coordinates\n", - "\n", - "```\n", - "(FS.positionRel, {'handL', 'handR'}, scale=[[0,0,1]], target=[0.1])\n", - "```\n", - "is the z-position position of the left hand measured in the frame of the right hand, with target 10centimeters.\n", - "\n", - "```\n", - "(FS.position, {'handL'}, order=1)\n", - "```\n", - "is the 3D velocity of the left hand in world coordinates\n", - "\n", - "```\n", - "(FS.scalarProductXX, {'handL', 'handR'}, target=[1])\n", - "```\n", - "says that the scalar product of the x-axes (e.g. directions of the index finger) of both hands should equal 1, which means they are aligned.\n", - "\n", - "```\n", - "(FS.scalarProductXY, {'handL', 'handR'})\n", - "(FS.scalarProductXZ, {'handL', 'handR'})\n", - "```\n", - "says that the the x-axis of handL should be orthogonal (zero scalar product) to the y- and z-axis of handR. So this also describes aligning both x-axes. However, this formulation is much more robust, as it has good error gradients around the optimum." - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Objectives\n", - "\n", - "Features are meant to define objectives in an optimization problem. An objective is\n", - "* a feature\n", - "* an indicator $\\rho_k\n", - "\\in\\{\\texttt{ineq, eq, sos}\\}$ that states whether the features\n", - "implies an inequality, an equality, or a sum-of-square objective\n", - "* and an index tuple $\\pi_k \\subseteq \\{1,..,n\\}$ that states which\n", - "configurations this feature is defined over.\n", - "\n", - "Then, given a set\n", - "$\\{\\phi_1,..,\\phi_K\\}$ of $K$ features, and a set $\\{x_1,..,x_n\\}$ of\n", - "$n$ configurations, this defines the mathematical program\n", - "\n", - "\\begin{align}\n", - " \\min_{x_1,..,x_n} \\sum_{k : \\rho_k=\\texttt{sos}} \\phi_k(x_{\\pi_k})^T \\phi_k(x_{\\pi_k})\n", - " ~\\text{s.t.}~ \\mathop\\forall_{k : \\rho_k=\\texttt{ineq}} \\phi_k(x_{\\pi_k}) \\le 0 ~,\\quad\n", - " \\mathop\\forall_{k : \\rho_k=\\texttt{eq}} \\phi_k(x_{\\pi_k}) = 0 ~,\\quad\n", - "\\end{align}" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Code example for collision features\n", - "\n", - "* Get list of collisions and proximities for the whole configuration\n", - "* Get a accumulative, differentiable collision measure\n", - "* Get proximity/penetration specifically for a pair of shapes\n", - "* Other geometric collision features for a pair of shapes (witness points, normal, etc) -- all differentiable" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import sys\n", - "sys.path.append('../build')\n", - "import numpy as np\n", - "import libry as ry\n", - "\n", - "C = ry.Config()\n", - "C.addFile('../rai-robotModels/pr2/pr2.g');\n", - "C.addFile('../rai-robotModels/objects/kitchen.g');\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Let's evaluate the accumulative collision scalar and its Jacobian" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "coll = C.feature(ry.FS.accumulatedCollisions, [])\n", - "\n", - "C.computeCollisions() #collisions/proxies are not automatically computed on set...State\n", - "coll.eval(C)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Let's move into collision and redo this" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C.selectJointsByTag([\"base\"])\n", - "C.setJointState([1.5,1,0])\n", - "\n", - "C.computeCollisions()\n", - "coll.eval(C)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "We can get more verbose information like this:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C.getCollisions()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C.getCollisions(0) #only report proxies with distance<0 (penetrations)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The computeCollisions() method calls a collision detection engine (SWIFT++) for the whole configuration, checking all shapes that are collision-activated. The activation/deactivation of collision computations is a nuissance! the 'contact' flag in g-files specifies which shapes are activated by default, and if the value is negative, that collisions with parent shapes are not included. (In the KOMO class, you can use activateCollisionPairs and deactivateCollisionPairs to modify these defaults in optimization problems... TODO: also in Config)\n", - "\n", - "When you're interested in the distance or penetration of one specific pair of objects, you don't need to call computeCollisions() and instead query a feature that calls the GJK (and others) algorithm directly only for this pair:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "dist = C.feature(ry.FS.distance, ['coll_wrist_r', '_10'])\n", - "dist.eval(C)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Note that this returns the NEGATIVE distance (because one typically wants to put an inequality (<=0) on this). The C++ code implements many more features of the collision geometry, including the normal, witness points, etc. Can be added to python easily on request." - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.6.9" - } - }, - "nbformat": 4, - "nbformat_minor": 2 -} diff --git a/notebooks/retired/3-IK-optimization.ipynb b/notebooks/retired/3-IK-optimization.ipynb deleted file mode 100644 index f9a2da2b7..000000000 --- a/notebooks/retired/3-IK-optimization.ipynb +++ /dev/null @@ -1,378 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Inverse Kinematics Optimization\n", - "\n", - "The previous doc explained features and how they define objectives of a constrained optimization problem. Here we show how to use this to solve IK optimization problems.\n", - "\n", - "At the bottom there is more general text explaining the basic concepts." - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Demo of features in Inverse Kinematics\n", - "\n", - "Let's setup a standard configuration. (Lock the window with \"Always on Top\".)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import sys\n", - "sys.path.append('../build')\n", - "import numpy as np\n", - "import libry as ry" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C = ry.Config()\n", - "C.addFile('../rai-robotModels/pr2/pr2.g')\n", - "C.addFile('../rai-robotModels/objects/kitchen.g')\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "For simplicity, let's add a frame that represents goals" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "goal = C.addFrame(\"goal\")\n", - "goal.setShape(ry.ST.sphere, [.05])\n", - "goal.setColor([.5,1,1])\n", - "goal.setPosition([1,.5,1])\n", - "X0 = C.getFrameState() #store the initial configuration\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "We create an IK engine. The only objective is that the `positionDiff` (position difference in world coordinates) between `pr2L` (the yellow blob in the left hand) and `goal` is equal to zero:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "IK = C.komo_IK(False)\n", - "IK.addObjective([], type=ry.OT.eq, feature=ry.FS.positionDiff, frames=['pr2L', 'goal'])" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "We now call the optimizer (True means with random initialization/restart)." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "ret = ry.NLP_Solver()\\\n", - " .setProblem(IK.nlp()) \\\n", - " .solve()\n", - "print(ret)\n", - "IK.getReport()\n", - "IK.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "One way to retrieve the result is to copy the frame state of the optimized IK configuration back into your working configuration C" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C.setFrameState( IK.getFrameState(0) )\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "We can redo the optimization, but for a different configuration, namely a configuration where the goal is in another location.\n", - "For this we move goal in our working configuration C, then copy C back into the IK engine's configurations:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "## (iterate executing this cell for different goal locations!)\n", - "\n", - "# move goal\n", - "goal.setPosition([.8,.2,.5])\n", - "\n", - "# copy C into the IK's internal configuration(s)\n", - "IK.initOrg()\n", - "\n", - "# reoptimize\n", - "print( ry.NLP_Solver() .setProblem(IK.nlp()) .solve() )\n", - "\n", - "# grab result\n", - "C.setFrameState( IK.getFrameState(0) )\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Let's solve some other problems, always creating a novel IK engine:\n", - "\n", - "The relative position of `goal` in `pr2R` coordinates equals [0,0,-.2] (which is 20cm straight in front of the yellow blob)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C.setFrameState(X0)\n", - "IK = C.komo_IK(False)\n", - "IK.addObjective([],type=ry.OT.eq, feature=ry.FS.positionRel, frames=['goal','pr2R'], target=[0,0,-.2])\n", - "print( ry.NLP_Solver() .setProblem(IK.nlp()) .solve() )\n", - "C.setFrameState( IK.getFrameState(0) )\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The distance between `pr2R` and `pr2L` is zero:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C.setFrameState(X0)\n", - "IK = C.komo_IK(False)\n", - "IK.addObjective([], type=ry.OT.eq, feature=ry.FS.distance, frames=['pr2L','pr2R'])\n", - "print( ry.NLP_Solver() .setProblem(IK.nlp()) .solve() )\n", - "C.setFrameState( IK.getFrameState(0) )\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The 3D difference between the z-vector of `pr2R` and the z-vector of `goal`:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C.setFrameState(X0)\n", - "IK = C.komo_IK(False)\n", - "IK.addObjective([], type=ry.OT.eq, feature=ry.FS.vectorZDiff, frames=['pr2R', 'goal'])\n", - "IK.optimize()\n", - "C.setFrameState( IK.getConfiguration(0) )" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The scalar product between the z-vector of `pr2R` and the z-vector of `goal` is zero:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C.setFrameState(X0)\n", - "IK = C.komo_IK(False)\n", - "IK.addObjective([], type=ry.OT.eq, feature=ry.FS.scalarProductZZ, frames=['pr2R', 'goal'])\n", - "print( ry.NLP_Solver() .setProblem(IK.nlp()) .solve() )\n", - "C.setFrameState( IK.getFrameState(0) )" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "etc etc" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## More explanations\n", - "\n", - "All methods to compute paths or configurations solve constrained optimization problems. To use them, you need to learn to define constrained optimization problems. Some definitions:\n", - "\n", - "* An objective defines either a sum-of-square cost term, or an equality constraint, or an inequality constraint in the optimization problem. An objective is defined by its type and its feature. The type can be `sos` (sum-of-squares), `eq`, or `ineq`, referring to the three cases.\n", - "* A feature is a (differentiable mapping) from the decision variable (the full path, or robot configuration) to a feature space. If the feature space is, e.g., 3-dimensional, this defines 3 sum-of-squares terms, or 3 inequality, or 3 equality constraints, one for each dimension. For instance, the feature can be the 3-dim robot hand position in the 15th time slice of a path optimization problem. If you put an equality constraint on this feature, then this adds 3 equality constraints to the overall path optimization problem.\n", - "* A feature is defined by the keyword for the feature map (e.g., `pos` for position), typically by a set of frame names that tell which objects we refer to (e.g., `pr2L` for the left hand of the pr2), optionally some modifiers (e.g., a scale or target, which linearly transform the feature map), and the set of configuration IDs or time slices the feature is to be computed from (e.g., `confs=[15]` if the feat is to be computed from the 15th time slice in a path optimization problem).\n", - "* In path optimization problems, we often want to add objectives for a whole time interval rather than for a single time slice or specific configuration. E.g., avoid collisions from start to end. When adding objectives to the optimization problem we can specify whole intervals, with `times=[1., 2.]`, so that the objective is added to each configuration in this time interval.\n", - "* Some features, especially velocity and acceleration, refer to a tuple of (consecutive) configurations. E.g., when you impose an acceleration feature, you need to specify `confs=[13, 14, 15]`. Or if you use `times=[1.,1.]`, the acceleration features is computed from the configuration that corresponds to time=1 and the two configurations *before*.\n", - "* All kinematic feature maps (that depend on only one configuration) can be modified to become a velocity or acceleration features. E.g., the position feature map can be modified to become a velocity or acceleration feature.\n", - "* The `sos`, `eq`, and `ineq` always refer to the feature map to be *zero*, e.g., constraining all features to be equal to zero with `eq`. This is natural for many features, esp. when they refer to differences (e.g. `posDiff` or `posRel`, which compute the relative position between two frames). However, when one aims to constrain the feature to a non-zero constant value, one can modify the objective with a `target` specification.\n", - "* Finally, all features can be rescaled with a `scale` specification. Rescaling changes the costs that arise from `sos` objectives. Rescaling also has significant impact on the convergence behavior for `eq` and `ineq` constraints. As a guide: scale constraints so that if they *would* be treated as squared penalties (squaredPenalty optim mode, to be made accessible) convergence to reasonable approximate solutions is efficient. Then the AugLag will also converge efficiently to precise constraints." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "## Designing a cylinder grasp" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "D=0\n", - "C=0" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import sys\n", - "sys.path.append('../build')\n", - "import numpy as np\n", - "import libry as ry" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C = ry.Config()\n", - "C.addFile('../rai-robotModels/pr2/pr2.g')\n", - "C.addFile('../rai-robotModels/objects/kitchen.g')\n", - "C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C.setJointState([.7], [\"l_gripper_l_finger_joint\"])\n", - "C.setJointState( C.getJointState() )" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "goal = C.addFrame(\"goal\")\n", - "goal.setShape(ry.ST.cylinder, [0,0,.2, .03])\n", - "goal.setColor([.5,1,1])\n", - "goal.setPosition([1,.5,1])\n", - "X0 = C.getFrameState()\n", - "C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C.setFrameState(X0)\n", - "goal.setPosition([1,.5,1.2])\n", - "IK = C.komo_IK(False)\n", - "IK.addObjective([], type=ry.OT.eq, feature=ry.FS.positionDiff, frames=['pr2L', 'goal'], scale=[[1,0,0],[0,1,0]])\n", - "IK.addObjective([], type=ry.OT.ineq, feature=ry.FS.positionDiff, frames=['pr2L', 'goal'], scale=[[0,0,1]], target=[0,0,.05])\n", - "IK.addObjective([], type=ry.OT.ineq, feature=ry.FS.positionDiff, frames=['pr2L', 'goal'], scale=[[0,0,-1]], target=[0,0,-.05])\n", - "IK.addObjective([], type=ry.OT.sos, feature=ry.FS.scalarProductZZ, frames=['pr2L', 'goal'], scale=[0.1])\n", - "IK.addObjective([], type=ry.OT.eq, feature=ry.FS.scalarProductXZ, frames=['pr2L', 'goal'])\n", - "print( ry.NLP_Solver() .setProblem(IK.nlp()) .solve() )\n", - "C.setFrameState( IK.getFrameState(0) )\n", - "C.view()\n", - "IK.getReport()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.6.9" - } - }, - "nbformat": 4, - "nbformat_minor": 2 -} diff --git a/notebooks/retired/4-LGP.ipynb b/notebooks/retired/4-LGP.ipynb deleted file mode 100644 index 28dbda723..000000000 --- a/notebooks/retired/4-LGP.ipynb +++ /dev/null @@ -1,241 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# 4. Logic-Geometric Programming: Skeleton Interface\n", - "\n", - "* Formally, in papers, a skeleton is defined by the sequence $a_{1:K}$ of symbolic decisions. Note that, given the decision rules, such a sequence also defines the sequence of logical state $s_{1:K}$. Practially, in the code, we define a skeleton to be a simple data structure that captures the relevant information from both, $a_{1:K}$ and $s_{1:K}$.\n", - "* A skeleton is a list of tuples $(t_\\text{start}, t_\\text{end}, \\text{literal})$, where $t_\\text{start}$ denotes the time (actually real-valued phase, but typically the integer k) at which a certain literal becomes true, $t_\\text{end}$ denotes the time when the literal was deleted, and `literal` is a some literal, e.g. `(stable hand object)`. In our case, the literals only include symbols (no real-valued constants), the first of which is the predicate, and the others typically refer to objects.\n", - "* The skeleton interface to LGP means that you can define a skeleton, and let the algorithm compute a corresponding path. Note that this does not solve the search over symbolic decisions. It allows you to specify the path constraints on the same abstract level that is used internally for LGP.\n", - "* Currently implemented skeleton predicates are:\n", - "\n", - "| predicate | arguments | description |\n", - "|---|---|---|\n", - "| touch | (obj1, obj2) | zero distance equality |\n", - "| above | (obj, table) | center of object vertically inside convex table |\n", - "| inside | (obj, box) | center of object inside convex box |\n", - "| impulse | (obj1, obj2) | impule exchange equation |\n", - "| stable | (from, to) | stable but free (7dof) relation |\n", - "| stableOn | (from, to) | stable constrained (3dof xyphi) relation |\n", - "| dynamic | (from, to) | free dynamics and corresponding equation |\n", - "| dynamicOn | (from, to) | constrained 3dof dynamics |\n", - "| liftDownUp | | |\n", - "| push | | |\n", - "| graspSlide| | |\n", - "\n", - "* Generally, a skeleton is translated to a set of objectives (and kinematic switches, and other internal things) for a path or configuration optimization problem. Note that this translation is not unique: In particular, for the same skeleton we can compute different *bounds*, each of which corresponds to a different optimization problem. In `bounds.h` in the `LGP/` module several mappings from a skeleton to an optimization problem are defined. These differ in terms of the objective set, but in terms of parameters like the resolution of the path discretization.\n", - "* To summarize, in LGP a decision sequence $a_{1:K}$ maps to a skeleton $S$ -- this mapping is defined by the decision rules which state which literals are created by which decisions (typically defined in `fol.g`). And then the skeleton $S$ is mapped to a constrained optimization problem (as set of objectives) -- this mapping is defined by the bound." - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "metadata": {}, - "outputs": [], - "source": [ - "import sys\n", - "sys.path.append('../ry')\n", - "from libry import *\n", - "from numpy import *" - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "metadata": {}, - "outputs": [], - "source": [ - "K = Config()\n", - "K.addFile(\"../test/boxProblem.g\")" - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "metadata": {}, - "outputs": [], - "source": [ - "komo = K.komo_path(5., 10, .2)\n", - "\n", - "obj = 'ballR'\n", - "\n", - "# this is all yet 'magic' -> clearer interface\n", - "komo.addTimeOptimization();\n", - "komo.makeObjectsFree([obj]);\n", - "komo.addObjective(type=OT.sos, feature=FS.accumulatedCollisions)\n", - "\n", - "S = [\n", - " [1,5], [\"dynamicTrans\", obj],\n", - " [2,2], [\"bounce\", \"boxBo\", obj],\n", - " [3,3], [\"bounce\", \"boxBo\", obj],\n", - " [4,4], [\"bounce\", \"boxBo\", obj],\n", - " [5,5], [\"touch\", \"target\", obj] ]\n", - "\n", - "komo.addSkeleton(S)" - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "metadata": {}, - "outputs": [], - "source": [ - "komo.optimize();" - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "metadata": {}, - "outputs": [], - "source": [ - "komoView = komo.view()" - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "metadata": {}, - "outputs": [], - "source": [ - "komoView=0\n", - "komo=0\n", - "D=0\n", - "K=0" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "\n", - "\n", - "\n" - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "metadata": {}, - "outputs": [], - "source": [ - "K = Config()\n", - "K.addFile(\"../test/lgp-example.g\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "We're creating the same skeleton that'd be created by the decision sequence\n", - "```\n", - "(grasp baxterR stick) (handover baxterR stick baxterL) (hitSlide stickTip redBall table1) (graspSlide baxterR redBall table1)\n", - "```\n", - "which is a standard demo in the RSS'18 paper" - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "metadata": {}, - "outputs": [], - "source": [ - "komo = K.komo_path(4.5, 10, 10.)\n", - "komo.addObjective(type=OT.sos, feature=FS.transAccelerations, scale=[1])" - ] - }, - { - "cell_type": "code", - "execution_count": 9, - "metadata": {}, - "outputs": [], - "source": [ - "S = [\n", - "#(grasp baxterR stick)\n", - " [1,1], [\"touch\", \"baxterR\", \"stick\"],\n", - " [1,1], [\"stable\", \"baxterR\", \"stick\"],\n", - " [1,1], [\"liftDownUp\", \"baxterR\"],\n", - "#(handover baxterR stick baxterL)\n", - " [2,2], [\"touch\", \"baxterL\", \"stick\"],\n", - " [2,4], [\"stable\", \"baxterL\", \"stick\"],\n", - "#(hitSlide stickTip redBall table1)\n", - " [3,3], [\"touch\", \"stickTip\", \"redBall\"],\n", - " [3,3], [\"impulse\", \"stickTip\", \"redBall\"],\n", - " [3,3], [\"dynamicOn\", \"table1\", \"redBall\"],\n", - "#(graspSlide baxterR redBall table1)\n", - " [4,4], [\"graspSlide\", \"baxterR\", \"redBall\", \"table1\"] ]" - ] - }, - { - "cell_type": "code", - "execution_count": 10, - "metadata": {}, - "outputs": [], - "source": [ - "komo.addSkeletonBound(S, BT.path)" - ] - }, - { - "cell_type": "code", - "execution_count": 11, - "metadata": {}, - "outputs": [], - "source": [ - "komo.optimize()" - ] - }, - { - "cell_type": "code", - "execution_count": 12, - "metadata": {}, - "outputs": [], - "source": [ - "komoView = komo.view()" - ] - }, - { - "cell_type": "code", - "execution_count": 13, - "metadata": {}, - "outputs": [], - "source": [ - "komoView=0\n", - "komo=0\n", - "D=0\n", - "K=0" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.5.2" - } - }, - "nbformat": 4, - "nbformat_minor": 2 -} diff --git a/notebooks/retired/4-path-optimization.ipynb b/notebooks/retired/4-path-optimization.ipynb deleted file mode 100644 index 462de5630..000000000 --- a/notebooks/retired/4-path-optimization.ipynb +++ /dev/null @@ -1,149 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Path Optimization (KOMO)\n", - "\n" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Let's setup a standard configuration. (Lock the window with \"Always on Top\".)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import sys\n", - "sys.path.append('../build')\n", - "import numpy as np\n", - "import libry as ry\n", - "\n", - "C = ry.Config()\n", - "C.view()\n", - "C.addFile('../rai-robotModels/pr2/pr2.g')\n", - "C.addFile('../rai-robotModels/objects/kitchen.g')" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Let's add some objects" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C.addObject(name='item1', parent='sink1', shape=ry.ST.ssBox, pos=[-.1, -.1, .52], size=[.1, .1, .25, .02], color=[1., 0., 0.])\n", - "C.addObject('item2', 'sink1', ry.ST.ssBox, [.1, .1, .25, .02], [1., 1., 0.], [.1, .1, .52])\n", - "C.addObject('tray', 'stove1', ry.ST.ssBox, [.2, .2, .05, .02], [0., 1., 0.], [.0, .0, .42])\n", - "C.view()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "compute a collision free path to touch object item1 with pr2L:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "X0 = C.getFrameState()\n", - "\n", - "obj1 = \"item1\";\n", - "arm = \"pr2R\";\n", - "\n", - "komo = C.komo_path(1.,20, 10., True);\n", - "\n", - "komo.addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.eq);\n", - "komo.addObjective([], ry.FS.jointLimits, [], ry.OT.ineq);\n", - "komo.addObjective([1.], ry.FS.distance, [arm, obj1], ry.OT.eq, [1e2]);\n", - "komo.addObjective([.9,1.], ry.FS.positionDiff, [\"endeffWorkspace\", obj1], ry.OT.sos, [1e0]);\n", - "komo.addObjective(times=[1.], feature=ry.FS.qItself, type=ry.OT.eq, order=1);\n", - "\n", - "ry.NLP_Solver().setProblem(komo.nlp()).solve()\n", - "komo.getReport()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo.view(False, \"my solution\")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo.view_play(False, .1)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Best way to read out: grab a configuration into K and analyze it here:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C.setFrameState( komo.getFrameState(19) )\n", - "C.getJointState()\n", - "C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.6.9" - } - }, - "nbformat": 4, - "nbformat_minor": 2 -} diff --git a/notebooks/retired/5-cgo-optimization.ipynb b/notebooks/retired/5-cgo-optimization.ipynb deleted file mode 100644 index dd9573b8d..000000000 --- a/notebooks/retired/5-cgo-optimization.ipynb +++ /dev/null @@ -1,152 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Constrained Graph Optimization (KOMO)\n", - "\n", - "This is a dense optimization problem (in constrast to a path (k-order Markov) optimization). You can define arbitrary objectives across configurations." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import sys\n", - "sys.path.append('../build')\n", - "import numpy as np\n", - "import libry as ry\n", - "\n", - "C = ry.Config()\n", - "D = C.view()\n", - "C.addFile('../rai-robotModels/pr2/pr2.g')\n", - "C.addFile('../rai-robotModels/scenarios/kitchen.g')" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Let's add some objects" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C.addObject(name='item1', parent='sink1', shape=ry.ST.ssBox, pos=[-.1, -.1, .52], size=[.1, .1, .25, .02], color=[1., 0., 0.])\n", - "C.addObject('item2', 'sink1', ry.ST.ssBox, [.1, .1, .25, .02], [1., 1., 0.], [.1, .1, .52])\n", - "C.addObject('tray', 'stove1', ry.ST.ssBox, [.2, .2, .05, .02], [0., 1., 0.], [.0, .0, .42])" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "obj1 = \"item2\";\n", - "obj2 = \"item1\";\n", - "tray = \"tray\";\n", - "arm = \"pr2L\";\n", - "table = \"_12\";\n", - "\n", - "komo = C.komo_CGO(6, True)\n", - "\n", - "komo.activateCollisionPairs([(obj1, obj2)]);\n", - "komo.addObjective([], ry.OT.eq, ry.FS.accumulatedCollisions);\n", - "komo.addObjective([], ry.OT.ineq, ry.FS.jointLimits);\n", - "\n", - "komo.add_StableRelativePose([0, 1], arm, obj1);\n", - "komo.add_StableRelativePose([2, 3], arm, obj2);\n", - "komo.add_StableRelativePose([4, 5], arm, tray);\n", - "\n", - "komo.add_StableRelativePose([1,2,3,4,5], tray, obj1);\n", - "komo.add_StableRelativePose([3,4,5], tray, obj2);\n", - "\n", - "komo.add_StablePose([-1,0], obj1);\n", - "komo.add_StablePose([-1,0,1,2], obj2);\n", - "komo.add_StablePose([-1,0,1,2,3,4], tray);\n", - "\n", - "komo.add_grasp(0, arm, obj1);\n", - "komo.add_place(1, obj1, tray);\n", - "\n", - "komo.add_grasp(2, arm, obj2);\n", - "komo.add_place(3, obj2, tray);\n", - "\n", - "komo.add_grasp(4, arm, tray);\n", - "komo.add_place(5, tray, table);" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo.optimize(True)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "V = komo.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "V = 0" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Best way to read out: grab a configuration into K and analyze it there:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C.setFrameState( komo.getConfiguration(4) )\n", - "C.getJointState()" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.6.8" - } - }, - "nbformat": 4, - "nbformat_minor": 2 -} diff --git a/notebooks/retired/6-KOMO-physics.ipynb b/notebooks/retired/6-KOMO-physics.ipynb deleted file mode 100644 index 5dc07b1ef..000000000 --- a/notebooks/retired/6-KOMO-physics.ipynb +++ /dev/null @@ -1,190 +0,0 @@ -{ - "cells": [ - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import sys\n", - "sys.path.append('../build')\n", - "import numpy as np\n", - "import libry as ry" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# load and display a configuration\n", - "C = ry.Config()\n", - "C.addFile('../test/boxProblem.g')\n", - "D = C.view()\n", - "C.getFrameNames()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# create a standard path optimization problem with 4 phases, each of .2 seconds, and 10 steps\n", - "komo = C.komo_path(5., 10, .2, False)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# create tau-DOFs (time intervals) in the optimization problem and add respective constraints\n", - "komo.addTimeOptimization()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# take one object, and first 'switch' it to become freely movable without costs,\n", - "# then, starting at phase .7, make it dynamic (imposing NE equations)\n", - "obj = 'ballR'\n", - "#komo.addSwitch_magic(.0, 'base', obj)\n", - "komo.makeObjectsFree([\"ballR\"]);" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# (old) a way to constrain the object position to be 'before' the box before phase 0.2\n", - "#komo.addObjective(time=[0., .2], type=OT.ineq, feature=FS.position, frames=[obj], scaleTrans=[[0.,1.,0.]], target=[1.])" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# objectives for collision (permanent) and target (at phase-time 4.)\n", - "komo.addObjective(type=ry.OT.sos, feature=ry.FS.accumulatedCollisions)\n", - "komo.addObjective(time=[5.], type=ry.OT.eq, feature=ry.FS.distance, frames=['target', obj], scale=[3e1])" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# simple structure of this solution: bounce on the bottom of the box at times 1, 2, and 3\n", - "komo.addSwitch_dynamicTrans(1., -1., 'base', obj)\n", - "komo.addInteraction_elasticBounce(2., \"boxBo\", obj, .8)\n", - "komo.addInteraction_elasticBounce(3., \"boxBo\", obj, .8)\n", - "komo.addInteraction_elasticBounce(4., \"boxBo\", obj, .8)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo.optimize(True)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komoView = komo.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo.display()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# read out solution: the full frame path, the tau path (time optimization), list of interaction forces\n", - "obj_path = komo.getPathFrames([obj])\n", - "tau_path = komo.getPathTau()\n", - "forces = komo.getForceInteractions()\n", - "forces" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Conversion from steps to phase and time:\n", - "```\n", - "phase=0 is the FIXED initial configuration and corresponds to step=-1\n", - "step=0 is the first configuration s.t. optimization\n", - "\\tau(s) gives the time between step s-1 and step s\n", - "phase(step s) = (s+1)/stepsPerPhase\n", - "time(step s) = sum i=0:s tau(i)\n", - "```" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "D=0\n", - "komoView=0\n", - "komo=0\n", - "C=0" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.5.2" - } - }, - "nbformat": 4, - "nbformat_minor": 2 -} diff --git a/notebooks/retired/6-KOMO-skeleton.ipynb b/notebooks/retired/6-KOMO-skeleton.ipynb deleted file mode 100644 index 469a342ad..000000000 --- a/notebooks/retired/6-KOMO-skeleton.ipynb +++ /dev/null @@ -1,253 +0,0 @@ -{ - "cells": [ - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import sys\n", - "sys.path.append('../build')\n", - "import numpy as np\n", - "import libry as ry" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C = ry.Config()\n", - "C.addFile('../cpp/bouncingBall/boxProblem.g')" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "S = ry.Skeleton()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "obj = 'ballR'\n", - "S.addEntry([0., .5], ry.SY.magic, [obj])\n", - "S.addEntry([.7, 4.], ry.SY.dynamicTrans, [obj])\n", - "S.addEntry([1., 1.], ry.SY.bounce, [\"boxBo\", obj])\n", - "S.addEntry([2., 2.], ry.SY.bounce, [\"boxBo\", obj])\n", - "S.addEntry([3., 3.], ry.SY.bounce, [\"boxBo\", obj])\n", - "S.addEntry([4., 4.], ry.SY.touch, [\"target\", obj])" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo = S.getKomo_path(C, 20, 1., -1., 1e-2)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo.optimize(True)\n", - "komoView = komo.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# read out solution: the full frame path, the tau path (time optimization), list of interaction forces\n", - "obj_path = komo.getPathFrames([obj])\n", - "tau_path = komo.getPathTau()\n", - "forces = komo.getForceInteractions()\n", - "forces" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "D=0\n", - "komoView=0\n", - "komo=0\n", - "C=0" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C = ry.Config()\n", - "C.addFile(\"../rai-robotModels/pr2/pr2.g\")\n", - "C.addFile(\"../rai-robotModels/objects/tables.g\")\n", - "\n", - "C.addFrame(\"obj0\", \"table1\", \"type:ssBox size:[.1 .1 .2 .02] color:[1. 0. 0.], contact, logical={ object }, joint:rigid, Q:\" )\n", - "C.addFrame(\"obj1\", \"table1\", \"type:ssBox size:[.1 .1 .2 .02] color:[1. 0. 0.], contact, logical={ object }, joint:rigid, Q:\" )\n", - "C.addFrame(\"obj2\", \"table1\", \"type:ssBox size:[.1 .1 .2 .02] color:[1. 0. 0.], contact, logical={ object }, joint:rigid, Q:\" )\n", - "C.addFrame(\"obj3\", \"table1\", \"type:ssBox size:[.1 .1 .2 .02] color:[1. 0. 0.], contact, logical={ object }, joint:rigid, Q:\" )\n", - "C.addFrame(\"tray\", \"table2\", \"type:ssBox size:[.15 .15 .04 .02] color:[0. 1. 0.], logical={ table }, Q:\" );\n", - "C.addFrame(\"\", \"tray\", \"type:ssBox size:[.27 .27 .04 .02] color:[0. 1. 0.]\" )\n", - "C.view(False, \"initial model\")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "S = ry.Skeleton()\n", - "S.addEntry([1.], ry.SY.touch, ['pr2R', 'obj0'])\n", - "S.addEntry([1., 3.], ry.SY.stable, ['pr2R', 'obj0'])\n", - "S.addEntry([2.,2.], ry.SY.touch, ['pr2L', 'obj3'])\n", - "S.addEntry([2.,4.], ry.SY.stable, ['pr2L', 'obj3'])\n", - "S.addEntry([3.], ry.SY.above, ['obj0', 'tray'])\n", - "S.addEntry([3.,4.], ry.SY.stableOn, ['tray', 'obj0'])" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo = S.getKomo_waypoints(C, 1e-1, 1e-2)\n", - "komo.optimize(0)\n", - "komo.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo.view_play()\n", - "waypoints = komo.getPath_qAll()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo = S.getKomo_path(C, 10, .2,-1., 1e-2)\n", - "komo.initWithWaypoints(waypoints)\n", - "komo.optimize(0)\n", - "komo.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo.view_play()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "D=0\n", - "komoView=0\n", - "komo=0\n", - "C=0" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C = ry.Config()\n", - "C.addFile(\"../rai-robotModels/scenarios/RSSproblem-01.g\")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "S =[\n", - " [1, 1], ry.SY.touch, ['baxterR', 'stick'], \n", - "[1, 4], ry.SY.stable, ['baxterR', 'stick'], \n", - "[1, 1], ry.SY.liftDownUp, ['baxterR'], \n", - "[2, 2], ry.SY.touch, ['baxterL', 'stick'], \n", - "[2, 4], ry.SY.stable, ['baxterL', 'stick'], \n", - "[3, 3], ry.SY.touch, ['stickTip', 'redBall'], \n", - "[3, 3], ry.SY.impulse, ['stickTip', 'redBall'],\n", - "[3, 3], ry.SY.dynamicOn, ['table1', 'redBall'], \n", - "[4, 4], ry.SY.touch, ['baxterR', 'redBall'], \n", - "#[4, 5], ry.SY.stable, ['baxterR', 'redBall'], \n", - "[4, 5], ry.SY.graspSlide, ['baxterR', 'redBall', 'table1']\n", - "]" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo = C.komo_path(4., 10, 10., False)\n", - "komo.addSkeletonBound(S, ry.BT.path, False)\n", - "komo.optimize(True)\n", - "komoView = komo.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.6.9" - } - }, - "nbformat": 4, - "nbformat_minor": 2 -} diff --git a/notebooks/retired/9-robotModels.ipynb b/notebooks/retired/9-robotModels.ipynb deleted file mode 100644 index 68d442153..000000000 --- a/notebooks/retired/9-robotModels.ipynb +++ /dev/null @@ -1,142 +0,0 @@ -{ - "cells": [ - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# let's have a look at all robot models in rai-robotModels\n", - "import sys\n", - "sys.path.append('../build')\n", - "import numpy as np\n", - "import libry as ry" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "D = 0\n", - "C = ry.Config()\n", - "C.addFile('../rai-robotModels/pr2/pr2.g')\n", - "D = C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "D = 0\n", - "C = ry.Config()\n", - "C.addFile('../rai-robotModels/panda/panda.g')\n", - "D = C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "D = 0\n", - "C = ry.Config()\n", - "C.addFile('../rai-robotModels/baxter/baxter_new.g')\n", - "D = C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "D = 0\n", - "C = ry.Config()\n", - "C.addFile('../rai-robotModels/kuka_drake/setup.g')\n", - "D = C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "D = 0\n", - "C = ry.Config()\n", - "C.addFile('../rai-robotModels/objects/kitchen.g')\n", - "C.addFile('../rai-robotModels/objects/shelf.g')\n", - "C.addFile('../rai-robotModels/human_simple/human_simple.g')\n", - "D = C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "D = 0\n", - "C = ry.Config()\n", - "print('press ENTER in the window to animate; edit the file externally autoupdates the window; press Q in window to escape')\n", - "C.edit('../rai-robotModels/panda/panda.g')\n", - "D = C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "D = 0\n", - "C = ry.Config()\n", - "print(\"this will fail if you haven't called 'make -C ../rai-robotModels/bremenKitchen' to wget the binary mesh files\")\n", - "C.addFile('../rai-robotModels/bremenKitchen/bremenKitchen.g')\n", - "D = C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "D = 0" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.6.9" - } - }, - "nbformat": 4, - "nbformat_minor": 2 -} diff --git a/notebooks/retired/broken-6-KOMO-physics.ipynb b/notebooks/retired/broken-6-KOMO-physics.ipynb deleted file mode 100644 index b1b88327b..000000000 --- a/notebooks/retired/broken-6-KOMO-physics.ipynb +++ /dev/null @@ -1,190 +0,0 @@ -{ - "cells": [ - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import sys\n", - "sys.path.append('../build')\n", - "import numpy as np\n", - "import libry as ry" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# load and display a configuration\n", - "C = ry.Config()\n", - "C.addFile('../test/boxProblem.g')\n", - "D = C.view()\n", - "C.getFrameNames()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# create a standard path optimization problem with 4 phases, each of .2 seconds, and 10 steps\n", - "komo = C.komo_path(5., 10, .2, False)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# create tau-DOFs (time intervals) in the optimization problem and add respective constraints\n", - "komo.addTimeOptimization()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# take one object, and first 'switch' it to become freely movable without costs,\n", - "# then, starting at phase .7, make it dynamic (imposing NE equations)\n", - "obj = 'ballR'\n", - "#komo.addSwitch_magic(.0, 'base', obj)\n", - "komo.makeObjectsFree([\"ballR\"]);" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# (old) a way to constrain the object position to be 'before' the box before phase 0.2\n", - "#komo.addObjective(time=[0., .2], type=OT.ineq, feature=FS.position, frames=[obj], scaleTrans=[[0.,1.,0.]], target=[1.])" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# objectives for collision (permanent) and target (at phase-time 4.)\n", - "komo.addObjective(type=ry.OT.sos, feature=ry.FS.accumulatedCollisions)\n", - "komo.addObjective(time=[5.], type=ry.OT.eq, feature=ry.FS.distance, frames=['target', obj], scale=[3e1])" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# simple structure of this solution: bounce on the bottom of the box at times 1, 2, and 3\n", - "komo.addSwitch_dynamicTrans(1., -1., 'base', obj)\n", - "komo.addInteraction_elasticBounce(2., \"boxBo\", obj, .8)\n", - "komo.addInteraction_elasticBounce(3., \"boxBo\", obj, .8)\n", - "komo.addInteraction_elasticBounce(4., \"boxBo\", obj, .8)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo.optimize(True)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komoView = komo.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo.display()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# read out solution: the full frame path, the tau path (time optimization), list of interaction forces\n", - "obj_path = komo.getPathFrames([obj])\n", - "tau_path = komo.getPathTau()\n", - "forces = komo.getForceInteractions()\n", - "forces" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Conversion from steps to phase and time:\n", - "```\n", - "phase=0 is the FIXED initial configuration and corresponds to step=-1\n", - "step=0 is the first configuration s.t. optimization\n", - "\\tau(s) gives the time between step s-1 and step s\n", - "phase(step s) = (s+1)/stepsPerPhase\n", - "time(step s) = sum i=0:s tau(i)\n", - "```" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "D=0\n", - "komoView=0\n", - "komo=0\n", - "C=0" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.6.8" - } - }, - "nbformat": 4, - "nbformat_minor": 2 -} diff --git a/notebooks/retired/broken-6-KOMO-skeleton.ipynb b/notebooks/retired/broken-6-KOMO-skeleton.ipynb deleted file mode 100644 index 8bfecbd3d..000000000 --- a/notebooks/retired/broken-6-KOMO-skeleton.ipynb +++ /dev/null @@ -1,224 +0,0 @@ -{ - "cells": [ - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import sys\n", - "sys.path.append('../build')\n", - "import numpy as np\n", - "import libry as ry" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C = ry.Config()\n", - "C.addFile('../test/boxProblem.g')" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo = C.komo_path(4., 10, .2, False)\n", - "komo.addTimeOptimization()\n", - "\n", - "obj = 'ballR'\n", - "S = [\n", - " [0., .5], ry.SY.magic, [obj],\n", - " [.7, 4.], ry.SY.dynamicTrans, [obj],\n", - " [1., 1.], ry.SY.bounce, [\"boxBo\", obj],\n", - " [2., 2.], ry.SY.bounce, [\"boxBo\", obj],\n", - " [3., 3.], ry.SY.bounce, [\"boxBo\", obj],\n", - " [4., 4.], ry.SY.touch, [\"target\", obj]\n", - "]\n", - "komo.addSkeleton(S)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo.optimize(True)\n", - "komoView = komo.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# read out solution: the full frame path, the tau path (time optimization), list of interaction forces\n", - "obj_path = komo.getPathFrames([obj])\n", - "tau_path = komo.getPathTau()\n", - "forces = komo.getForceInteractions()\n", - "forces" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "D=0\n", - "komoView=0\n", - "komo=0\n", - "C=0" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C = ry.Config()\n", - "C.addFile(\"../rai-robotModels/pr2/pr2.g\")\n", - "C.addFile(\"../rai-robotModels/objects/tables.g\")\n", - "\n", - "C.addFrame(\"obj0\", \"table1\", \"type:ssBox size:[.1 .1 .2 .02] color:[1. 0. 0.], contact, logical={ object }, joint:rigid, Q:\" )\n", - "C.addFrame(\"obj1\", \"table1\", \"type:ssBox size:[.1 .1 .2 .02] color:[1. 0. 0.], contact, logical={ object }, joint:rigid, Q:\" )\n", - "C.addFrame(\"obj2\", \"table1\", \"type:ssBox size:[.1 .1 .2 .02] color:[1. 0. 0.], contact, logical={ object }, joint:rigid, Q:\" )\n", - "C.addFrame(\"obj3\", \"table1\", \"type:ssBox size:[.1 .1 .2 .02] color:[1. 0. 0.], contact, logical={ object }, joint:rigid, Q:\" )\n", - "C.addFrame(\"tray\", \"table2\", \"type:ssBox size:[.15 .15 .04 .02] color:[0. 1. 0.], logical={ table }, Q:\" );\n", - "C.addFrame(\"\", \"tray\", \"type:ssBox size:[.27 .27 .04 .02] color:[0. 1. 0.]\" )" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "S =[\n", - " [1., 1.], ry.SY.touch, ['pr2R', 'obj0'],\n", - " [1., 2.], ry.SY.stable, ['pr2R', 'obj0'],\n", - " [2.,2.], ry.SY.touch, ['pr2L', 'obj3'],\n", - " [2.,3.], ry.SY.stable, ['pr2L', 'obj3'],\n", - " [3.,3.], ry.SY.touch, ['pr2R', 'obj0'],\n", - " [3.,3.], ry.SY.above, ['obj0', 'tray'],\n", - " [3.,3.], ry.SY.stableOn, ['tray', 'obj0']\n", - "]" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo = C.komo_path(3., 10, 10., True)\n", - "komo.addSkeletonBound(S, ry.BT.seq, True)\n", - "komo.optimize(True)\n", - "komoView = komo.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo = C.komo_path(3., 10, 10., True)\n", - "komo.addSkeletonBound(S, ry.BT.path, True)\n", - "komo.optimize(True)\n", - "komoView = komo.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "D=0\n", - "komoView=0\n", - "komo=0\n", - "C=0" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C = ry.Config()\n", - "C.addFile(\"../rai-robotModels/scenarios/RSSproblem-01.g\")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "S =[\n", - " [1, 1], ry.SY.touch, ['baxterR', 'stick'], \n", - "[1, 4], ry.SY.stable, ['baxterR', 'stick'], \n", - "[1, 1], ry.SY.liftDownUp, ['baxterR'], \n", - "[2, 2], ry.SY.touch, ['baxterL', 'stick'], \n", - "[2, 4], ry.SY.stable, ['baxterL', 'stick'], \n", - "[3, 3], ry.SY.touch, ['stickTip', 'redBall'], \n", - "[3, 3], ry.SY.impulse, ['stickTip', 'redBall'],\n", - "[3, 3], ry.SY.dynamicOn, ['table1', 'redBall'], \n", - "[4, 4], ry.SY.touch, ['baxterR', 'redBall'], \n", - "#[4, 5], ry.SY.stable, ['baxterR', 'redBall'], \n", - "[4, 5], ry.SY.graspSlide, ['baxterR', 'redBall', 'table1']\n", - "]" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo = C.komo_path(4., 10, 10., False)\n", - "komo.addSkeletonBound(S, ry.BT.path, False)\n", - "komo.optimize(True)\n", - "komoView = komo.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.6.8" - } - }, - "nbformat": 4, - "nbformat_minor": 2 -} diff --git a/notebooks/retired/lgp1-pickAndPlace.ipynb b/notebooks/retired/lgp1-pickAndPlace.ipynb deleted file mode 100644 index 1078a3fa5..000000000 --- a/notebooks/retired/lgp1-pickAndPlace.ipynb +++ /dev/null @@ -1,316 +0,0 @@ -{ - "cells": [ - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import sys\n", - "sys.path.append('../build')\n", - "import numpy as np\n", - "import libry as ry" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "K = ry.Config()\n", - "K.addFile(\"../rai-robotModels/pr2/pr2.g\")\n", - "K.addFile(\"../rai-robotModels/objects/tables.g\")\n", - "\n", - "K.addFrame(\"obj0\", \"table1\", \"type:ssBox size:[.1 .1 .2 .02] color:[1. 0. 0.], contact, logical={ object }, joint:rigid, Q:\" )\n", - "K.addFrame(\"obj1\", \"table1\", \"type:ssBox size:[.1 .1 .2 .02] color:[1. 0. 0.], contact, logical={ object }, joint:rigid, Q:\" )\n", - "K.addFrame(\"obj2\", \"table1\", \"type:ssBox size:[.1 .1 .2 .02] color:[1. 0. 0.], contact, logical={ object }, joint:rigid, Q:\" )\n", - "K.addFrame(\"obj3\", \"table1\", \"type:ssBox size:[.1 .1 .2 .02] color:[1. 0. 0.], contact, logical={ object }, joint:rigid, Q:\" )\n", - "K.addFrame(\"tray\", \"table2\", \"type:ssBox size:[.15 .15 .04 .02] color:[0. 1. 0.], logical={ table }, Q:\" );\n", - "K.addFrame(\"\", \"tray\", \"type:ssBox size:[.27 .27 .04 .02] color:[0. 1. 0.]\" )\n", - "V = K.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "lgp = K.lgp(\"../rai/test/LGP/pickAndPlace/fol-pnp-switch.g\")\n", - "lgp.nodeInfo()\n", - "# this writes the initial state, which is important to check:\n", - "# do the grippers have the gripper predicate, do all objects have the object predicate, and tables the table predicate? These need to be set using a 'logical' attribute in the g-file\n", - "# the on predicate should automatically be generated based on the configuration" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "lgp.getDecisions()\n", - "# This is also useful to check: inspect all decisions possible in this node, which expands the node.\n", - "# If there is no good decisions, the FOL rules are buggy" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "lgp.walkToDecision(3)\n", - "lgp.nodeInfo()\n", - "# Using getDecisions and walkToDecision and walkToParent, you can walk to anywhere in the tree by hand" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "lgp.viewTree()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "lgp.walkToNode(\"(grasp pr2R obj0) (grasp pr2L obj1) (place pr2R obj0 tray)\")\n", - "lgp.nodeInfo()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# at a node, you can compute bounds, namely BT.seq (just key frames), BT.path (the full path),\n", - "# and BT.setPath (also the full path, but seeded with the BT.seq result)\n", - "lgp.optBound(ry.BT.seq, True)\n", - "lgp.nodeInfo()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo = lgp.getKOMOforBound(ry.BT.seq)\n", - "komo.display()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo = 0" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "lgp.optBound(ry.BT.path, True)\n", - "lgp.nodeInfo()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "lgp.viewTree()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# finally, the full multi-bound tree search (MBTS)\n", - "# you typically want to add termination rules, i.e., symbolic goals\n", - "print(\"THIS RUNS A THREAD. CHECK THE CONSOLE FOR OUTPUT. THIS IS GENERATING LOTS OF FILES.\")\n", - "lgp.addTerminalRule(\"(on obj0 tray) (on obj1 tray) (on obj2 tray)\")\n", - "lgp.run(2)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# wait until you have some number of solutions found (repeat executing this line...)\n", - "lgp.numSolutions()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# query the optimization features of the 0. solution\n", - "lgp.getReport(0, ry.BT.seqPath)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# get the KOMO object for the seqPath computation of the 0. solution\n", - "komo = lgp.getKOMO(0, ry.BT.seqPath)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo.displayTrajectory() #SOOO SLOOOW (TODO: add parameter for display speed)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# assign K to the 20. configuration of the 0. solution, check display\n", - "# you can now query anything (joint state, frame state, features)\n", - "X = komo.getConfiguration(20)\n", - "K.setFrameState(X) " - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "lgp.stop() #stops the thread... takes a while to finish the current job" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "lgp.run(2) #will continue where it stopped" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo=0\n", - "lgp=0" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import sys\n", - "sys.path.append('../rai/rai/ry')\n", - "import numpy as np\n", - "import libry as ry\n", - "\n", - "C = ry.Config()\n", - "D = C.view()\n", - "\n", - "C.addFile('../test/lgp-example.g');\n", - "\n", - "lgp = C.lgp(\"../test/fol.g\");" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "lgp.walkToNode(\"(grasp baxterR stick) (push stickTip redBall table1) (grasp baxterL redBall) \");\n", - "print(lgp.nodeInfo())" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "lgp.optBound(BT.pose, True);" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "komo = lgp.getKOMOforBound(BT.path)\n", - "komo.display()\n", - "\n", - "input(\"Press Enter to continue...\")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.7.5" - } - }, - "nbformat": 4, - "nbformat_minor": 2 -} diff --git a/notebooks/retired/sim1-physics.ipynb b/notebooks/retired/sim1-physics.ipynb deleted file mode 100644 index 072759913..000000000 --- a/notebooks/retired/sim1-physics.ipynb +++ /dev/null @@ -1,130 +0,0 @@ -{ - "cells": [ - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import sys\n", - "sys.path.append('../build')\n", - "import numpy as np\n", - "import libry as ry\n", - "from time import sleep" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C = ry.Config()\n", - "D = C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C.addFrame('world')\n", - "block = C.addObject(name='block', shape=ry.ST.ssBox, size=[.2,.3,.5,.02], pos=[.0,.0,.5], color=[1,0,0])\n", - "block.setMass(1.);\n", - "C.addObject('finger', shape=ry.ST.ssBox, size=[.3, .1, .1, .02], color=[1.,1.,1.,.3], parent='world', pos=[1., 0., .45])" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# this fails, if the lib was not compiled with bullet (comment #BULLET=0 in config.mk)\n", - "#P=C.physx()\n", - "P=C.bullet()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "x = C.getFrameState('finger')\n", - "for i in range(1,100):\n", - " x[0] -= .01\n", - " C.setFrameState(x, ['finger'])\n", - " P.step(C)\n", - " sleep(0.01)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "V = P.getState(C)\n", - "X = C.getFrameState()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "#run this cell multiple times! It resets bullet always to the same state\n", - "C.setFrameState(X)\n", - "x = C.getFrameState('finger')\n", - "P.setState(C,V)\n", - "for i in range(1,100):\n", - " x[0] -= .01\n", - " C.setFrameState(x, ['finger'])\n", - " P.step(C)\n", - " sleep(0.01)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "P=0\n", - "D=0\n", - "C=0" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.7.5" - } - }, - "nbformat": 4, - "nbformat_minor": 2 -} diff --git a/notebooks/script5-PathFind.ipynb b/notebooks/script5-PathFind.ipynb deleted file mode 100644 index 76e957c75..000000000 --- a/notebooks/script5-PathFind.ipynb +++ /dev/null @@ -1,227 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "48bcf763", - "metadata": {}, - "source": [ - "# Path finding example\n", - "* Path finding is using sample-based (RRT) methods rather than constrained optimization to find a collision free path\n", - "* Path finding is much easier if a final robot pose is given. We here use IK (formulated as KOMO problem) to first compute a final configuration 'qT'. (Path optimization does this jointly with optimizing the path.)\n", - "* Then we can pass the current state and qT to a bi-directional RRT to find a collision free path.\n", - "* Note that BotOp (the interface to sim/real) is opened only after we computed the motion. We simply pass the motion to be played by the sim/real." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "8e07bf36", - "metadata": {}, - "outputs": [], - "source": [ - "from robotic import ry\n", - "import time" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "059a8ee7", - "metadata": {}, - "outputs": [], - "source": [ - "C = ry.Config()\n", - "C.addFile(ry.raiPath('../rai-robotModels/scenarios/pandasTable.g'))\n", - "C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "582b68ba", - "metadata": {}, - "outputs": [], - "source": [ - "C.addFrame('boxR','table') \\\n", - " .setRelativePosition([.15,0,.1]) \\\n", - " .setShape(ry.ST.ssBox, size=[.1,.1,.1,.02]) \\\n", - " .setColor([1,1,0])\n", - "C.addFrame('boxL','table') \\\n", - " .setRelativePosition([-.15,0,.1]) \\\n", - " .setShape(ry.ST.ssBox, size=[.1,.1,.1,.02]) \\\n", - " .setColor([1,.5,0])\n", - "C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "eab44514", - "metadata": {}, - "outputs": [], - "source": [ - "# store the start configuration\n", - "q0 = C.getJointState()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "bccb7b55", - "metadata": {}, - "outputs": [], - "source": [ - "# compute a goal configuration\n", - "komo = ry.KOMO()\n", - "komo.setConfig(C, True)\n", - "komo.setTiming(1., 1, 5., 0)\n", - "komo.addControlObjective([], 0, 1e-0)\n", - "komo.addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.eq);\n", - "komo.addObjective([], ry.FS.jointLimits, [], ry.OT.ineq);\n", - "komo.addObjective([], ry.FS.positionDiff, ['r_gripper', 'boxL'], ry.OT.eq, [1e1]);\n", - "komo.addObjective([], ry.FS.positionDiff, ['l_gripper', 'boxR'], ry.OT.eq, [1e1]);" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "178e3d42", - "metadata": {}, - "outputs": [], - "source": [ - "ret = ry.NLP_Solver() \\\n", - " .setProblem(komo.nlp()) \\\n", - " .setOptions( stopTolerance=1e-2, verbose=4 ) \\\n", - " .solve()\n", - "print(ret)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "b727e37c", - "metadata": {}, - "outputs": [], - "source": [ - "# that's the goal configuration\n", - "qT = komo.getPath()[0]\n", - "C.setJointState(qT)\n", - "C.view(False, \"IK solution\")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "b20fc581", - "metadata": {}, - "outputs": [], - "source": [ - "#define a path finding problem\n", - "rrt = ry.PathFinder()\n", - "rrt.setProblem(C, [q0], [qT])" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "b0cde20c", - "metadata": {}, - "outputs": [], - "source": [ - "ret = rrt.solve()\n", - "print(ret)\n", - "path = ret.x" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "a615cc68", - "metadata": {}, - "outputs": [], - "source": [ - "# display the path\n", - "for t in range(0, path.shape[0]-1):\n", - " C.setJointState(path[t])\n", - " C.view()\n", - " time.sleep(.1)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "51d2e8e7", - "metadata": {}, - "outputs": [], - "source": [ - "# run the path with botop\n", - "C.setJointState(q0)\n", - "ry.params_add({'botsim/verbose': 1., 'physx/motorKp': 10000., 'physx/motorKd': 1000.})\n", - "bot = ry.BotOp(C, False)\n", - "bot.home(C)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "f0d8e62c", - "metadata": {}, - "outputs": [], - "source": [ - "bot.moveAutoTimed(path, 1., 1.)\n", - "while bot.getTimeToEnd()>0:\n", - " bot.sync(C, .1)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "9c02685e", - "metadata": {}, - "outputs": [], - "source": [ - "del bot" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "929345e5", - "metadata": {}, - "outputs": [], - "source": [ - "del rrt\n", - "del C" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "01b2e062", - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3 (ipykernel)", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/notebooks/sim1-physics.ipynb b/notebooks/sim1-physics.ipynb deleted file mode 100644 index 5b1bb3b03..000000000 --- a/notebooks/sim1-physics.ipynb +++ /dev/null @@ -1,141 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Direct simulation interface\n", - "\n", - "BotOp is a narrow control interface to a real or simulated robot, which is also real time and threaded (as for a real robot). However, sometimes we need a more low-level interface to a physical simulator, e.g. to implement a Reinforcement Learning environment.\n", - "TODO: explain simulation modes: all=kinematic, robot=kinematic + objects=physical, all=physical\n", - "TODO: PhysX multi-body mode (motors), gains, friction (gravity compensation?)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import sys\n", - "sys.path.append('../build')\n", - "import numpy as np\n", - "from robotic import ry\n", - "from time import sleep" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C = ry.Config()\n", - "D = C.view()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "C.addFrame('world')\n", - "block = C.addObject(name='block', shape=ry.ST.ssBox, size=[.2,.3,.5,.02], pos=[.0,.0,.5], color=[1,0,0])\n", - "block.setMass(1.);\n", - "C.addObject('finger', shape=ry.ST.ssBox, size=[.3, .1, .1, .02], color=[1.,1.,1.,.3], parent='world', pos=[1., 0., .45])" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# this fails, if the lib was not compiled with bullet (comment #BULLET=0 in config.mk)\n", - "#P=C.physx()\n", - "P=C.bullet()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "x = C.getFrameState('finger')\n", - "for i in range(1,100):\n", - " x[0] -= .01\n", - " C.setFrameState(x, ['finger'])\n", - " P.step(C)\n", - " sleep(0.01)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "V = P.getState(C)\n", - "X = C.getFrameState()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "#run this cell multiple times! It resets bullet always to the same state\n", - "C.setFrameState(X)\n", - "x = C.getFrameState('finger')\n", - "P.setState(C,V)\n", - "for i in range(1,100):\n", - " x[0] -= .01\n", - " C.setFrameState(x, ['finger'])\n", - " P.step(C)\n", - " sleep(0.01)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "P=0\n", - "D=0\n", - "C=0" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3 (ipykernel)", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 2 -} diff --git a/rai b/rai index d419d85c9..b8efb1fd6 160000 --- a/rai +++ b/rai @@ -1 +1 @@ -Subproject commit d419d85c9ad4c06243b2fdeb10c7756a506371b3 +Subproject commit b8efb1fd6f43d8eca707783d814cb9aad959b086 diff --git a/rai-robotModels b/rai-robotModels index c9e43aedc..65e4c7cfa 160000 --- a/rai-robotModels +++ b/rai-robotModels @@ -1 +1 @@ -Subproject commit c9e43aedc80c226c50b8f80f98d40d7603a86187 +Subproject commit 65e4c7cfab311431058de97ec062c81ad201c4ec diff --git a/robotic/render.py b/robotic/render.py new file mode 100644 index 000000000..857dd4e15 --- /dev/null +++ b/robotic/render.py @@ -0,0 +1,122 @@ +from robotic import ry +import numpy as np +import nvisii + +def quatMult(b, c): + a = b.copy(); + if(c[0]!=1.): + a[0] *= c[0]; a[1] *= c[0]; a[2] *= c[0]; a[3] *= c[0]; + if(c[1]!=0.): + a[0] -= b[1]*c[1]; a[1] += b[0]*c[1]; a[2] += b[3]*c[1]; a[3] -= b[2]*c[1]; + if(c[2]!=0.): + a[0] -= b[2]*c[2]; a[1] -= b[3]*c[2]; a[2] += b[0]*c[2]; a[3] += b[1]*c[2]; + if(c[3]!=0.): + a[0] -= b[3]*c[3]; a[1] += b[2]*c[3]; a[2] -= b[1]*c[3]; a[3] += b[0]*c[3]; + return a; + +def flipQuat(quat): + w = quat[0] + quat[0]=quat[1] + quat[1]=quat[2] + quat[2]=quat[3] + quat[3]=w + return quat + +class NvisiiRenderer: + def __init__(self, width, height, focalLength=1.): + nvisii.initialize(headless = True, verbose = True) + nvisii.enable_denoiser() + + self.width = width + self.height = height + + self.camera = nvisii.entity.create( + name = "camera", + transform = nvisii.transform.create(name = "camera_transform"), + camera = nvisii.camera.create_from_focal_length( + name = "camera_camera", + focal_length = focalLength * height, + sensor_width = width, + sensor_height = height + ) + ) + + nvisii.set_camera_entity(self.camera) + + nvisii.set_dome_light_intensity(.5) + nvisii.set_dome_light_color([1,1,1]) + # nvisii.disable_dome_light_sampling() + + def __del__(self): + nvisii.deinitialize() + + def addConfig(self, C): + frames = C.getFrameNames() + + for name in frames: + # print(name) + f = C.getFrame(name) + F = f.info() + if 'shape' in F and F['shape'] in ['mesh', 'ssBox']: + print('=== creating', name) + # print(F) + V = f.getMeshPoints() + T = f.getMeshTriangles() + col = F['color'] + if len(col) == 4: + continue + if len(col) == 1: + col = [col[0], col[0], col[0]] + pos = f.getPosition() + quat = f.getQuaternion() + + if not 'temperature' in F: + if col == None: + col = [.8, .5, .3] + obj = nvisii.entity.create( + name = name, + mesh = nvisii.mesh.create_from_data(name=name, positions=V.flatten(), indices=T.flatten()), + transform = nvisii.transform.create(name=name, position=pos, rotation=flipQuat(quat)), + material = nvisii.material.create(name) + ) + obj.get_material().set_base_color(col) + obj.get_material().set_roughness(0.7) + obj.get_material().set_specular(.5) + obj.get_material().set_sheen(.5) + + else: + print('=== creating light', name) + light = nvisii.entity.create( + name = name, + mesh = nvisii.mesh.create_from_data(name=name, positions=V.flatten(), indices=T.flatten()), + transform = nvisii.transform.create(name=name, position=pos, rotation=flipQuat(quat)), + light = nvisii.light.create(f'{name}_light') + ) + light.get_light().set_intensity(1.) + light.get_light().set_exposure(3) + # light.get_light().set_color(col) + light.get_light().set_temperature(F['temperature']) + # light.get_light().use_surface_area(True) + + + def setCamera(self, C: ry.Config): + X = C.view_pose() + pos = X[:3] + quat = X[3:] + quat = quatMult(quat, [0,1,0,0]) + quat = quat / np.linalg.norm(quat) + self.camera.get_transform().set_position(pos) + self.camera.get_transform().set_rotation(flipQuat(quat)) + + def render(self, samples=64, flip=True): + print('rendering...') + # nvisii.render_to_file(self.width, self.height, 50, '01_simple_scene.png') + buffer = nvisii.render(self.width, self.height, samples) + img = np.array(buffer, dtype='float32').reshape(self.height, self.width, -1) + img = img[:,:,:3] + rgb = np.clip(255.*img, 0, 255).astype(np.uint8) + if flip: + rgb = np.flip(rgb, axis=0) + return rgb + + diff --git a/robotic/ry-view b/robotic/ry-view new file mode 100755 index 000000000..f5cf82873 --- /dev/null +++ b/robotic/ry-view @@ -0,0 +1,24 @@ +#!/usr/bin/python3 + +from robotic import ry +import argparse + +parser = argparse.ArgumentParser( + description='View a g-file (robotic model description) during editing') + +parser.add_argument('FILE', type=str, + help='g-file name') +parser.add_argument('-v', '--version', action='version', + version=f'%(prog)s -- robotic package version: {ry.__version__}, {ry.compiled()}') + +def main(): + args = parser.parse_args() + + try: + C = ry.Config() + C.watchFile(args.FILE) + except KeyboardInterrupt: + sys.exit(1) + +if __name__ == "__main__": + main() diff --git a/robotic/version.py b/robotic/version.py index 23a88d087..7de336049 100644 --- a/robotic/version.py +++ b/robotic/version.py @@ -1 +1 @@ -__version__ = '0.0.22' +__version__ = '0.0.23' diff --git a/setup.py b/setup.py index 82943b0cc..f980d9abb 100644 --- a/setup.py +++ b/setup.py @@ -30,10 +30,10 @@ def has_ext_modules(foo): setup( name='robotic', packages=['robotic'], - package_data={'robotic': ['ry.so', 'ry.pyi', 'version.py', 'rai-robotModels/*/*', 'rai-robotModels/*/*/*', 'rai-robotModels/*/*/*/*', 'rai-robotModels/*/*/*/*/*', 'rai-robotModels/*/*/*/*/*/*']}, + package_data={'robotic': ['ry.so', 'ry.pyi', 'version.py', 'render.py', 'rai-robotModels/*/*', 'rai-robotModels/*/*/*', 'rai-robotModels/*/*/*/*', 'rai-robotModels/*/*/*/*/*', 'rai-robotModels/*/*/*/*/*/*']}, include_package_data=True, # cmdclass={ 'build_ext': CustomCommand }, - scripts=['rai/bin/urdf2rai.py'], + scripts=['rai/bin/urdf2rai.py', 'robotic/ry-view'], description="Robotic Control Interface & Manipulation Planning Library", long_description=long_description, From 446adfb29d8f3595bf27edf5ccbb2d15e9c91eb3 Mon Sep 17 00:00:00 2001 From: Marc Toussaint Date: Thu, 19 Oct 2023 15:39:49 +0200 Subject: [PATCH 2/7] render install --- _build_utils/CMakeLists-ubuntu.txt | 2 +- rai | 2 +- robotic/render.py | 10 +++++++--- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/_build_utils/CMakeLists-ubuntu.txt b/_build_utils/CMakeLists-ubuntu.txt index 3206c112c..9973edc32 100644 --- a/_build_utils/CMakeLists-ubuntu.txt +++ b/_build_utils/CMakeLists-ubuntu.txt @@ -213,7 +213,7 @@ install( DESTINATION ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITE}/robotic) install( - FILES robotic/__init__.py robotic/version.py ${CMAKE_BINARY_DIR}/ry.pyi + FILES robotic/__init__.py robotic/version.py robotic/render.py ${CMAKE_BINARY_DIR}/ry.pyi DESTINATION ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITE}/robotic) install( diff --git a/rai b/rai index b8efb1fd6..47f73898d 160000 --- a/rai +++ b/rai @@ -1 +1 @@ -Subproject commit b8efb1fd6f43d8eca707783d814cb9aad959b086 +Subproject commit 47f73898d6f3628307bbfafc6190ed3e19fc7982 diff --git a/robotic/render.py b/robotic/render.py index 857dd4e15..b69ae6fbf 100644 --- a/robotic/render.py +++ b/robotic/render.py @@ -48,6 +48,7 @@ def __init__(self, width, height, focalLength=1.): # nvisii.disable_dome_light_sampling() def __del__(self): + print('-- shutting down Nvisii') nvisii.deinitialize() def addConfig(self, C): @@ -65,12 +66,12 @@ def addConfig(self, C): col = F['color'] if len(col) == 4: continue - if len(col) == 1: - col = [col[0], col[0], col[0]] pos = f.getPosition() quat = f.getQuaternion() if not 'temperature' in F: + if len(col) == 1: + col = [col[0], col[0], col[0]] if col == None: col = [.8, .5, .3] obj = nvisii.entity.create( @@ -92,7 +93,10 @@ def addConfig(self, C): transform = nvisii.transform.create(name=name, position=pos, rotation=flipQuat(quat)), light = nvisii.light.create(f'{name}_light') ) - light.get_light().set_intensity(1.) + if len(col) == 1: + light.get_light().set_intensity(col[0]) + else: + light.get_light().set_intensity(1.) light.get_light().set_exposure(3) # light.get_light().set_color(col) light.get_light().set_temperature(F['temperature']) From 83c946a3951e5ec2415ac280eb0ea96f7ded3cb5 Mon Sep 17 00:00:00 2001 From: Marc Toussaint Date: Fri, 20 Oct 2023 16:24:02 +0200 Subject: [PATCH 3/7] v24 --- _build_utils/CMakeLists-ubuntu.txt | 2 +- _build_utils/build-wheels.sh | 2 +- botop | 2 +- rai | 2 +- robotic/version.py | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/_build_utils/CMakeLists-ubuntu.txt b/_build_utils/CMakeLists-ubuntu.txt index 9973edc32..4afef87f9 100644 --- a/_build_utils/CMakeLists-ubuntu.txt +++ b/_build_utils/CMakeLists-ubuntu.txt @@ -183,7 +183,7 @@ message(STATUS "[rai] compiler flags: " "${_defs}") add_custom_target(docstrings #ALL DEPENDS ry - COMMAND pybind11-stubgen ry + COMMAND pybind11-stubgen --ignore-invalid=all ry COMMAND mv stubs/ry*/__init__.pyi ry.pyi) add_executable(kinEdit rai/bin/src_kinEdit/main.cpp) diff --git a/_build_utils/build-wheels.sh b/_build_utils/build-wheels.sh index c889ff18f..321715cdd 100755 --- a/_build_utils/build-wheels.sh +++ b/_build_utils/build-wheels.sh @@ -36,7 +36,7 @@ for ver in 8 9 10 6 7; do cp -f build/ry.*3$ver*.so robotic/ry.so cp -f build/stubs/ry*/__init__.pyi robotic/ry.pyi python3.$ver setup.py bdist_wheel - break + #break done echo -e "\n\n======== renaming wheels ========" diff --git a/botop b/botop index a519ffd74..e1fa6552a 160000 --- a/botop +++ b/botop @@ -1 +1 @@ -Subproject commit a519ffd7462e9be3ae3d140baeb0b85c0db08b9f +Subproject commit e1fa6552a2a41cbf52a7370f35a26697e40b5d70 diff --git a/rai b/rai index 47f73898d..ebff925d6 160000 --- a/rai +++ b/rai @@ -1 +1 @@ -Subproject commit 47f73898d6f3628307bbfafc6190ed3e19fc7982 +Subproject commit ebff925d6757201c68f15288851ec68378e43f9f diff --git a/robotic/version.py b/robotic/version.py index 7de336049..8c7e5dbf2 100644 --- a/robotic/version.py +++ b/robotic/version.py @@ -1 +1 @@ -__version__ = '0.0.23' +__version__ = '0.0.24' From 529d2eed99222e10e797298d26ced4a657a7ecef Mon Sep 17 00:00:00 2001 From: Marc Toussaint Date: Wed, 25 Oct 2023 09:44:58 +0200 Subject: [PATCH 4/7] docs as submodule --- .github/workflows/cmake.yml | 25 ++++++++----------------- .github/workflows/sphinx.yml | 30 ++++++++++++++++++++++++++++++ .gitmodules | 3 +++ Makefile | 5 +++++ _build_utils/build-wheels.sh | 3 ++- rai-docs | 1 + rai-robotModels | 2 +- 7 files changed, 50 insertions(+), 19 deletions(-) create mode 100644 .github/workflows/sphinx.yml create mode 100644 Makefile create mode 160000 rai-docs diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml index a20a619bb..20430e210 100644 --- a/.github/workflows/cmake.yml +++ b/.github/workflows/cmake.yml @@ -8,38 +8,29 @@ env: jobs: build: - # The CMake configure and build commands are platform agnostic and should work equally - # well on Windows or Mac. You can convert this to a matrix build if you need - # cross-platform coverage. - # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: - # recursively checkout all submodules submodules: 'recursive' - + - uses: actions/setup-python@v3 + with: + python-version: '3.10' - name: install ubuntu dependencies run: | sudo apt-get install libboost-system-dev libfcl-dev APTGETYES=1 make -C rai -j1 installUbuntuAll - - - name: install python and pybind + - name: install python dependencies run: | - sudo apt-get install python3 python3-dev python3-numpy python3-pip python3-distutils - sudo pip3 install jupyter nbconvert matplotlib pybind11 - + pip install numpy pybind11 - name: setup build files run: | - make -C rai -j1 unityAll ln -s build_utils/CMakeLists-ubuntu.txt CMakeLists.txt cd $HOME && mkdir -p .local/include && cd .local/include && ln -s /usr/include/libqhull qhull - - name: cmake configure run: | export PYTHONVERSION=`python3 -c "import sys; print(str(sys.version_info[0])+'.'+str(sys.version_info[1]))"` - cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DUSE_PHYSX=OFF -DUSE_BULLET=OFF -DPYBIND11_PYTHON_VERSION=$PYTHONVERSION -DUSE_QHULL8=ON - + ls + cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DUSE_PHYSX=OFF -DUSE_BULLET=OFF -DPYBIND11_PYTHON_VERSION=$PYTHONVERSION -DUSE_QHULL8=ON . - name: cmake build run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} diff --git a/.github/workflows/sphinx.yml b/.github/workflows/sphinx.yml new file mode 100644 index 000000000..ca9d66114 --- /dev/null +++ b/.github/workflows/sphinx.yml @@ -0,0 +1,30 @@ +name: SphinxDocumentation + +on: [push, pull_request] + +permissions: + contents: write + +jobs: + docs: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + with: + submodules: 'recursive' + - uses: actions/setup-python@v3 + with: + python-version: '3.10' + - name: install ubuntu dependencies + run: | + sudo apt-get install pandoc freeglut3 libglew-dev + - name: install python dependencies + run: | + pip install sphinx sphinx_rtd_theme myst_parser nbsphinx robotic numpy + - name: Sphinx build + run: | + cd rai-docs && sphinx-build doc ../html + - name: Deploy to GitHub Pages + uses: JamesIves/github-pages-deploy-action@v4 + with: + folder: html diff --git a/.gitmodules b/.gitmodules index 97e6d6105..44a027480 100644 --- a/.gitmodules +++ b/.gitmodules @@ -7,3 +7,6 @@ [submodule "botop"] path = botop url = git@github.com:MarcToussaint/botop.git +[submodule "rai-docs"] + path = rai-docs + url = git@github.com:MarcToussaint/rai-docs.git diff --git a/Makefile b/Makefile new file mode 100644 index 000000000..8f8889103 --- /dev/null +++ b/Makefile @@ -0,0 +1,5 @@ +docs: + cd rai-docs && sphinx-build doc ../html + +clean: + rm -Rf build diff --git a/_build_utils/build-wheels.sh b/_build_utils/build-wheels.sh index 321715cdd..2dbdbc9c1 100755 --- a/_build_utils/build-wheels.sh +++ b/_build_utils/build-wheels.sh @@ -13,7 +13,8 @@ rm -Rf rai-robotModels mkdir rai-robotModels cd rai-robotModels mkdir -p objects; cp ../../rai-robotModels/objects/*.g objects -mkdir -p panda; cp ../../rai-robotModels/panda/*.g panda; cp -R ../../rai-robotModels/panda/meshes panda +mkdir -p panda; cp ../../rai-robotModels/panda/*.g panda; cp -R ../../rai-robotModels/panda/*_description panda +mkdir -p ur10; cp ../../rai-robotModels/ur10/*.g ur10; cp -R ../../rai-robotModels/ur10/*_description ur10 mkdir -p pr2; cp ../../rai-robotModels/pr2/*.g pr2; cp -R ../../rai-robotModels/pr2/meshes pr2 mkdir -p baxter; cp ../../rai-robotModels/baxter/*.g baxter; cp -R ../../rai-robotModels/baxter/*_description baxter mkdir -p robotiq; cp ../../rai-robotModels/robotiq/*.g robotiq; cp -R ../../rai-robotModels/robotiq/meshes robotiq diff --git a/rai-docs b/rai-docs new file mode 160000 index 000000000..4c027ab3d --- /dev/null +++ b/rai-docs @@ -0,0 +1 @@ +Subproject commit 4c027ab3d1c1b88c30c97e9b09eee7e0395f7618 diff --git a/rai-robotModels b/rai-robotModels index 65e4c7cfa..ebd16bc7b 160000 --- a/rai-robotModels +++ b/rai-robotModels @@ -1 +1 @@ -Subproject commit 65e4c7cfab311431058de97ec062c81ad201c4ec +Subproject commit ebd16bc7b65f294e8485f35405201a94ecdb73d4 From b1c6761811964f6401d7bdcabd6b761c84101ddc Mon Sep 17 00:00:00 2001 From: Marc Toussaint Date: Wed, 25 Oct 2023 18:04:45 +0200 Subject: [PATCH 5/7] v25 - big cleanup --- .gitignore | 3 ++- Makefile | 43 ++++++++++++++++++++++++++++-- README.md | 11 ++++---- _build_utils/CMakeLists-docker.txt | 34 +++++++++++------------ _build_utils/CMakeLists-ubuntu.txt | 41 ++++++++++++++-------------- _build_utils/Dockerfile | 1 + _build_utils/build-wheels.sh | 27 ++++++++++--------- _build_utils/run-docker.sh | 12 ++++----- rai | 2 +- robotic/version.py | 2 +- setup.py | 4 ++- 11 files changed, 113 insertions(+), 67 deletions(-) diff --git a/.gitignore b/.gitignore index 3429962a5..9f2d8256f 100644 --- a/.gitignore +++ b/.gitignore @@ -3,9 +3,10 @@ dist/ robotic.egg-info/ rai-robotModels ry.pyi +build_wheel/ %% compile temporaries -/build +build/ *.o *.a *.so diff --git a/Makefile b/Makefile index 8f8889103..4827ccc9d 100644 --- a/Makefile +++ b/Makefile @@ -1,5 +1,44 @@ +default: docs + docs: cd rai-docs && sphinx-build doc ../html -clean: - rm -Rf build +docs-clean: + rm -Rf html + +install: + ln -f -s _build_utils/CMakeLists-ubuntu.txt CMakeLists.txt + cmake -B build . + +cmake --install build + +install-clean: + rm -R ${HOME}/.local/lib/python3.8/site-packages/robotic* + rm ${HOME}/.local/lib/*rai* + rm ${HOME}/.local/bin/*ry* + +wheels: + $(eval id = $(shell _build_utils/run-docker.sh -d)) + @echo "started docker " ${id} + docker exec -it ${id} /bin/bash -C local/_build_utils/build-wheels.sh + docker stop ${id} +# _build_utils/run-docker.sh local/_build_utils/build-wheels.sh + +wheels-push: + twine upload dist/*.whl --repository robotic + +wheels-local: + python3.8 -m pip install --user dist/robotic-*cp38*.whl --force-reinstall + +test: + cd ${HOME} && python3 -c 'from robotic import ry; print("ry version:", ry.__version__, ry.compiled());' + +pull: + cd rai && git pull + cd rai-robotModels && git pull + cd rai-docs && git pull + cd botop && git pull + +docker-clean: + $(shell docker container kill "$(docker container ls -q)") + docker system prune + diff --git a/README.md b/README.md index 7813919dd..868704a6b 100644 --- a/README.md +++ b/README.md @@ -14,7 +14,7 @@ Lab](https://argmin.lis.tu-berlin.de/), operate our robots. ## Documentation -Please follow the documentation and tutorials here: https://marctoussaint.github.io/robotics-course/ +Please follow the documentation and tutorials here: https://marctoussaint.github.io/robotic/ ## Installation via pip (simulation only, no real Franka & realsense support) @@ -24,11 +24,13 @@ sudo apt install liblapack3 freeglut3 libglew-dev python3 python3-pip ``` * pip-install robotic and dependencies (numpy, scipy) ``` -python3 -m pip install --user robotic numpy scipy +pip install robotic ``` -* Test: +* Tests: ``` python3 -c 'from robotic import ry; print("ry version:", ry.__version__, ry.compiled());' +``` +``` python3 -c 'from robotic import ry; ry.test.RndScene()' ```