{ "cells": [ { "cell_type": "markdown", "id": "94bf0959", "metadata": {}, "source": [ "# KOMO-3: Manipulation Modelling & Execution\n", "\n", "The discussed components (KOMO, BotOp, NLP_Solver, RRT) provide basic ingredients for manipulation planning and execution. This tutorial is about how to practically use these in typical manipulation settings.\n", "\n", "The first focus is on *manipulation modelling*. While KOMO provides a very powerful abstract framework to define all kinds of constraints, here we discuss what are concrete useful constraints for typical actions, e.g., picking and placing a box, or capsule. The *ManipulationModelling* class is meant to translate between typical actions and the abstract KOMO specification of the corresponding constraints.\n", "\n", "The second focus is on the whole pipeline. We follow a basic sense-plan-act pipeline (not yet a fully integrated reactive framework such as SecMPC). To be more detailed, we assume the following basic steps in each loop:\n", "* Perception: Update the configuration model to be in sync with the real world - using perception.\n", "* Discrete decisions (task planning): Decide on discrete next actions, such as which object to pick or place next.\n", "* Waypoint planning: Model the manipulation constraints for the next few actions and solve them to get a plan for the next few waypoints.\n", "* Path planning: Create a fine-grained path/trajectory between waypoints, sometimes justing quick interpolation & optimization, sometimes using full fledge path finding (bi-directional RRT).\n", "* Execution: Sending the path to BotOp for running it on the real system.\n", "\n", "We neglect perception and discrete decision making here." ] }, { "cell_type": "markdown", "id": "aa8bba6a", "metadata": {}, "source": [ "## Manipulation Modelling\n", "\n", "We start with discussing manipulation modelling for standard box/cylinder grasping and placing." ] }, { "cell_type": "code", "execution_count": null, "id": "a13d9bd0", "metadata": {}, "outputs": [], "source": [ "import robotic as ry\n", "import numpy as np\n", "import random\n", "import time" ] }, { "cell_type": "markdown", "id": "b913c478", "metadata": {}, "source": [ "A basic configuration with a box and cylinder:" ] }, { "cell_type": "code", "execution_count": null, "id": "caf624e0", "metadata": {}, "outputs": [], "source": [ "C = ry.Config()\n", "C.addFile(ry.raiPath('scenarios/pandaSingle.g'))\n", "\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(1)\n", "\n", "C.addFrame('capsule') \\\n", " .setShape(ry.ST.capsule, [.2,.02]) \\\n", " .setPosition([.25,.1,1.]) \\\n", " .setColor([1,.5,0]) \\\n", " .setContact(1)\n", "\n", "# for convenience, a few definitions:\n", "qHome = C.getJointState()\n", "gripper = 'l_gripper'\n", "palm = 'l_palm'\n", "box = 'box'\n", "table = 'table'\n", "boxSize = C.getFrame(box).getSize()\n", "\n", "C.view()" ] }, { "cell_type": "markdown", "id": "c7f564ac", "metadata": {}, "source": [ "Look into the definition of *ManipulationModelling* class! You see that this class provides wrapper methods to setup a komo problem. The following demonstrate the methods provided to model box and cylinder grasping IK problems:" ] }, { "cell_type": "markdown", "id": "2ab84a9b", "metadata": {}, "source": [ "### Box centered top grasp\n", "There are 6 possible orientation of an orthonormal centered box grasp. Have a look at the `grasp_top_box` method!" ] }, { "cell_type": "code", "execution_count": null, "id": "e16fa08c", "metadata": {}, "outputs": [], "source": [ "C.setJointState(qHome)\n", "for orientation in ['xy', 'xz', 'yx', 'yz', 'zx', 'zy']: #loops over the 6 possible grasps\n", " # setup the manipulation problem\n", " man = ry.KOMO_ManipulationHelper()\n", " man.setup_inverse_kinematics(C)\n", " man.grasp_top_box(1., gripper, box, orientation)\n", " \n", " # solve it\n", " ret = man.solve()\n", " \n", " # check feasibility and display\n", " if ret.feasible:\n", " C.setJointState(man.path[0])\n", " C.view(True, f'grasp_top_box with orientation {orientation}\\nret: {ret}')\n", " else:\n", " print(' -- infeasible')" ] }, { "cell_type": "markdown", "id": "b73a260d", "metadata": {}, "source": [ "### Box general grasp\n", "We do not have to grasp a box in the center or orthonormally. We can only specify along which axis the fingers should press, and that they need to be inside a margin of the box sides. Have a look at the `grasp_box` method! To illustrate the gained degrees of freedom, we also impose a random bias (leading to different solutions in nullspace):" ] }, { "cell_type": "code", "execution_count": null, "id": "dd066bb7", "metadata": {}, "outputs": [], "source": [ "C.setJointState(qHome)\n", "limits = C.getJointLimits()\n", "for orientation in ['x', 'y', 'z']:\n", " for i in range(10):\n", " # setup the manipulation problem\n", " man = ry.KOMO_ManipulationHelper()\n", " man.setup_inverse_kinematics(C)\n", " # ... with random bias in joint space\n", " qBias = limits[0]+np.random.uniform(qHome.shape)%(limits[1]-limits[0])\n", " man.bias(1., qBias, 1e0)\n", " # ... and general, non-centered box grasping\n", " man.grasp_box(1., gripper, box, palm, orientation, margin=.02)\n", " \n", " # solve\n", " ret = man.solve()\n", " \n", " # if feasible, display\n", " if ret.feasible:\n", " C.setJointState(man.path[0])\n", " C.view(True, f'grasp_box with orientation {orientation}\\nret: {ret}')\n", " else:\n", " print('-- infeasible', i, orientation)" ] }, { "cell_type": "markdown", "id": "2b4bc84d", "metadata": {}, "source": [ "### Cylinder grasp\n", "\n", "A cylinder (or capsule) can be grasped by ensuring the finger axis is normal to the cylinder's axis -- have a look at the `grasp_cylinder` method. Again, a demo with random bias to show the variety of grasps modelled that way:" ] }, { "cell_type": "code", "execution_count": null, "id": "301f497d", "metadata": {}, "outputs": [], "source": [ "C.setJointState(qHome)\n", "limits = C.getJointLimits()\n", "for i in range(10):\n", " # setup the manipulation problem\n", " man = ry.KOMO_ManipulationHelper()\n", " man.setup_inverse_kinematics(C)\n", " qBias = limits[0]+np.random.uniform(qHome.shape)%(limits[1]-limits[0])\n", " man.bias(1., qBias, 1e0)\n", " man.grasp_cylinder(1., gripper, 'capsule', palm)\n", " \n", " # solve\n", " ret = man.solve()\n", " \n", " # if feasible, display\n", " if ret.feasible:\n", " C.setJointState(man.path[0])\n", " C.view(True, f'grasp_cylinder\\nret: {ret}')\n", " else:\n", " print('-- infeasible', i, orientation)" ] }, { "cell_type": "markdown", "id": "61b864f4", "metadata": {}, "source": [ "## Sequential Manipulation Modelling\n", "\n", "Sequential manipulation modelling is special, as in some phases the manipulated objects move with the manipulator. Internally, komo models this with a *mode switch* (where an object becomes attached to the manipulator with a stable (but optimizable) relative transform).\n", "\n", "Using the ManipulationModelling class, the `setup_pick_and_place_waypoints` method creates a two-time-steps komo problem where the relative object-gripper position is constrained to be the same in the 1st and 2st step (as it is parameterized by a shared relative kinematic joint). The `grasp_box` method ensures that the solution *also* fulfils grasp constraints in the first time step; and the `place_box` method ensures that the solution *also* fulfils placement constraints in the second time step. The additional `target_relative_xy_position` is optional, so see placement to explicit xy-positions on the table. Have a look at the definitions of all these methods." ] }, { "cell_type": "code", "execution_count": null, "id": "cbf6f327", "metadata": {}, "outputs": [], "source": [ "C.setJointState(qHome)\n", "\n", "for i in range(10):\n", " grasp_ori = random.choice(['x', 'y', 'z'])\n", " place_ori = 'z' #random.choice(['x', 'y', 'z'])\n", " info = f'pnp {i}, grasp orientation {grasp_ori}, place orientation {place_ori}'\n", " print('===', info)\n", " \n", " # setup manipulation problem\n", " man = ry.KOMO_ManipulationHelper()\n", " man.setup_pick_and_place_waypoints(C, gripper, box)\n", " man.grasp_box(1., gripper, box, palm, grasp_ori)\n", " man.place_box(2., box, table, palm, place_ori)\n", " man.target_relative_xy_position(2., box, table, [(i%5)*.1-.2, .2])\n", " \n", " # solve\n", " ret = man.solve()\n", "\n", " # if feasible, display (including 'fake' simulation with kinematic attach)\n", " if ret.feasible:\n", " C.setJointState(man.path[0])\n", " C.view(True, f'{info}\\nwaypoint 0\\nret: {ret}')\n", " C.attach(gripper, box)\n", " C.setJointState(man.path[1])\n", " C.view(True, f'{info}\\nwaypoint 1\\nret: {man.ret}')\n", " C.attach(table, box)\n", " C.setJointState(qHome)\n", " C.view(True, 'back home')\n", " else:\n", " print(' -- infeasible')\n", "\n", "del man\n", "C.getFrame('box').setPosition([-.25,.1,1.])\n", "C.view()" ] }, { "cell_type": "markdown", "id": "9c96d8a9", "metadata": {}, "source": [ "## Path generation\n", "\n", "Once solutions to the manipulation keyframes/waypoints are available, the next step is to generate motion between them. We can use sample-based path finding (bi-directional RRT) and/or smooth motion optimization for this." ] }, { "cell_type": "markdown", "id": "36c75c59", "metadata": {}, "source": [ "### Smooth point-to-point motion\n", "The following demonstrates smooth point-to-point motion between box grasps, there the motion is additionally constrains the endeffector to retract and approach:" ] }, { "cell_type": "code", "execution_count": null, "id": "5a241da0", "metadata": {}, "outputs": [], "source": [ "C.setJointState(qHome)\n", "limits = C.getJointLimits()\n", "verbose = 0\n", "\n", "for i in range(20):\n", " qStart = C.getJointState()\n", " \n", " # choose a random grasp orientation\n", " orientation = random.choice(['x', 'y', 'z'])\n", " print('===', i, 'orientation', orientation)\n", " \n", " # setup the grasp problem\n", " man = ry.KOMO_ManipulationHelper()\n", " man.setup_inverse_kinematics(C, accumulated_collisions=True)\n", " man.grasp_box(1., gripper, box, palm, orientation)\n", " \n", " # solve\n", " ret = man.solve()\n", " path = man.path\n", " print(' IK:', ret)\n", " \n", " # if feasible, display; otherwise try another grasp\n", " if ret.feasible:\n", " if verbose>0:\n", " C.setJointState(man.path[0])\n", " C.view(True, f'grasp {i} with orientation {orientation}\\nret: {ret}')\n", " else:\n", " print(' -- infeasible')\n", " C.setJointState(qStart)\n", " continue\n", "\n", " # setup the motion problem\n", " man = ry.KOMO_ManipulationHelper()\n", " man.setup_point_to_point_motion(C, path[0])\n", " man.retract([.0, .2], gripper)\n", " # man.approach([.8, 1.], gripper)\n", " \n", " # solve\n", " ret = man.solve()\n", " print(' path:', ret)\n", "\n", " # if feasible, display trivially (no real execution in BotOp here)\n", " if ret.feasible:\n", " for t in range(man.path.shape[0]):\n", " C.setJointState(man.path[t])\n", " C.view(False, f'grasp {i} with orientation {orientation}, path step {t}\\n{ret}')\n", " time.sleep(.05)\n", " C.view(verbose>0, f'path done')\n", " else:\n", " print(' -- infeasible')\n", " \n", "del man" ] }, { "cell_type": "markdown", "id": "bebd5d0e", "metadata": {}, "source": [ "## Integrated Example\n", "\n", "Let's start with an integrated example, where the robot endlessly loops through picking and placing a box on a table." ] }, { "cell_type": "code", "execution_count": null, "id": "8d6ad421", "metadata": {}, "outputs": [], "source": [ "import robotic as ry\n", "import manipulation as manip\n", "import numpy as np\n", "#from importlib import reload\n", "import time\n", "import random" ] }, { "cell_type": "markdown", "id": "1bebb1bf", "metadata": {}, "source": [ "We define a basic configuration with box on the table:" ] }, { "cell_type": "code", "execution_count": null, "id": "dc1db72a", "metadata": {}, "outputs": [], "source": [ "C = ry.Config()\n", "C.addFile(ry.raiPath('../rai-robotModels/scenarios/pandaSingle.g'))\n", "\n", "C.addFrame('box', 'table') \\\n", " .setJoint(ry.JT.rigid) \\\n", " .setShape(ry.ST.ssBox, [.15,.06,.06,.005]) \\\n", " .setRelativePosition([-.0,.3-.055,.095]) \\\n", " .setContact(1) \\\n", " .setMass(.1)\n", "\n", "C.addFrame('obstacle', 'table') \\\n", " .setShape(ry.ST.ssBox, [.06,.15,.06,.005]) \\\n", " .setColor([.1]) \\\n", " .setRelativePosition([-.15,.3-.055,.095]) \\\n", " .setContact(1)\n", "\n", "C.delFrame('panda_collCameraWrist')\n", "\n", "# for convenience, a few definitions:\n", "qHome = C.getJointState()\n", "gripper = 'l_gripper'\n", "palm = 'l_palm'\n", "box = 'box'\n", "table = 'table'\n", "boxSize = C.getFrame(box).getSize()\n", "\n", "C.view()" ] }, { "cell_type": "markdown", "id": "34495bc3", "metadata": {}, "source": [ "### top grasps and pick-and-place over an object" ] }, { "cell_type": "code", "execution_count": null, "id": "fb96e54c", "metadata": {}, "outputs": [], "source": [ "#reload(manip)\n", "\n", "C.setJointState(qHome)\n", "C.view_raise()\n", "\n", "C.getFrame(box).setRelativePosition([-.0,.3-.055,.095])\n", "C.getFrame(box).setRelativeQuaternion([1.,0,0,0])\n", "\n", "for i in range(7):\n", " qStart = C.getJointState()\n", "\n", " graspDirection = 'yz' #random.choice(['xz', 'yz'])\n", " placeDirection = 'z'\n", " place_position = [(i%3)*.3-.3, .2]\n", " place_orientation = [-(i%2),((i+1)%2),0.]\n", " info = f'placement {i}: grasp {graspDirection} place {placeDirection} place_pos {place_position} place_ori {place_orientation}'\n", " print('===', info)\n", "\n", " M = ry.KOMO_ManipulationHelper()\n", " M.setup_pick_and_place_waypoints(C, gripper, box, homing_scale=1e-1, joint_limits=False)\n", " M.grasp_top_box(1., gripper, box, graspDirection)\n", " M.place_box(2., box, table, palm, placeDirection)\n", " M.target_relative_xy_position(2., box, table, place_position)\n", " M.target_x_orientation(2., box, place_orientation)\n", " M.solve()\n", " if not M.feasible:\n", " continue\n", "\n", " M2 = M.sub_motion(0)\n", " M2.retract([.0, .2], gripper)\n", " M2.approach([.8, 1.], gripper)\n", " M2.solve()\n", " if not M2.ret.feasible:\n", " continue\n", "\n", " M3 = M.sub_motion(1)\n", "# M3.retract([.0, .2], box, distance=.05)\n", "# M3.approach([.8, 1.], box, distance=.05)\n", " M3.no_collisions([], [table, box])\n", " M3.no_collisions([], [box, 'obstacle'])\n", " M3.bias(.5, qHome, 1e0)\n", " M3.solve()\n", " if not M3.ret.feasible:\n", " continue\n", " \n", " M2.play(C)\n", " C.attach(gripper, box)\n", " M3.play(C)\n", " C.attach(table, box)\n", "\n", "del M" ] }, { "cell_type": "markdown", "id": "63b89383", "metadata": {}, "source": [ "### endless box pick and place with random pick and place orientations" ] }, { "cell_type": "code", "execution_count": null, "id": "34ec695e", "metadata": {}, "outputs": [], "source": [ "#reload(manip)\n", "\n", "C.delFrame('obstacle')\n", "\n", "C.setJointState(qHome)\n", "C.view_raise()\n", "\n", "for l in range(20):\n", " qStart = C.getJointState()\n", "\n", " graspDirection = random.choice(['y', 'z']) #'x' not possible: box too large\n", " placeDirection = random.choice(['x', 'y', 'z', 'xNeg', 'yNeg', 'zNeg'])\n", " info = f'placement {l}: grasp {graspDirection} place {placeDirection}'\n", " print('===', info)\n", "\n", " M = ry.KOMO_ManipulationHelper(info)\n", " M.setup_pick_and_place_waypoints(C, gripper, box, homing_scale=1e-1)\n", " M.grasp_box(1., gripper, box, palm, graspDirection)\n", " M.place_box(2., box, table, palm, placeDirection)\n", " M.no_collisions([], [palm, table])\n", " M.target_relative_xy_position(2., box, table, [.2, .3])\n", " ways = M.solve()\n", "\n", " if not M.feasible:\n", " continue\n", "\n", " M2 = M.sub_motion(0)\n", " # M = ry.KOMO_ManipulationHelper(C, info, helpers=[gripper])\n", " # M.setup_point_to_point_motion(qStart, ways[0])\n", " M2.no_collisions([.3,.7], [palm, box], margin=.05)\n", " M2.retract([.0, .2], gripper)\n", " M2.approach([.8, 1.], gripper)\n", " M2.solve()\n", " if not M2.feasible:\n", " continue\n", "\n", " M3 = M.sub_motion(1)\n", " #ry.KOMO_ManipulationHelper(C, info)\n", " # M.setup_point_to_point_motion(ways[0], ways[1])\n", " M3.no_collisions([], [table, box])\n", " M3.solve()\n", " if not M3.ret.feasible:\n", " continue\n", "\n", " M2.play(C)\n", " C.attach(gripper, box)\n", " M3.play(C)\n", " C.attach(table, box)\n", "\n", "del M" ] }, { "cell_type": "markdown", "id": "1f08fc42", "metadata": {}, "source": [ "### random pushes" ] }, { "cell_type": "code", "execution_count": null, "id": "70f07626", "metadata": {}, "outputs": [], "source": [ "#from importlib import reload \n", "#reload(ry)\n", "\n", "C.getFrame('l_panda_finger_joint1').setJointState(np.array([.01]))\n", "\n", "obj = box\n", "C.getFrame(obj).setRelativePosition([-.0,.3-.055,.095])\n", "C.getFrame(obj).setRelativeQuaternion([1.,0,0,0])\n", "\n", "for i in range(10):\n", " qStart = C.getJointState()\n", "\n", " info = f'push {i}'\n", " print('===', info)\n", "\n", " M = ry.KOMO_ManipulationHelper(info)\n", " M.setup_sequence(C, 2, 1e-1, accumulated_collisions=False)\n", " M.komo.addFrameDof('obj_trans', table, ry.JT.transXY, False, obj) #a permanent moving(!) transXY joint table->trans, and a snap trans->obj\n", " M.komo.addRigidSwitch(1., ['obj_trans', obj])\n", " pushStart = M.straight_push([1.,2.], obj, gripper, table)\n", " #random target position\n", " M.komo.addObjective([2.], ry.FS.position, [obj], ry.OT.eq, 1e1*np.array([[1,0,0],[0,1,0]]), .4*np.random.rand(3) - .2+np.array([.0,.3,.0]))\n", " M.solve()\n", " if not M.ret.feasible:\n", " continue\n", "\n", " M1 = M.sub_motion(0, accumulated_collisions=False)\n", " M1.retractPush([.0, .15], gripper, .03)\n", " M1.approachPush([.85, 1.], gripper, .03)\n", " M1.no_collisions([.15,.85], [obj, 'l_finger1'], .02)\n", " M1.no_collisions([.15,.85], [obj, 'l_finger2'], .02)\n", " M1.no_collisions([.15,.85], [obj, 'l_palm'], .02)\n", " M1.no_collisions([], [table, 'l_finger1'], .0)\n", " M1.no_collisions([], [table, 'l_finger2'], .0)\n", " M1.solve()\n", " if not M1.ret.feasible:\n", " continue\n", "\n", " M2 = M.sub_motion(1, accumulated_collisions=False)\n", " #M2.komo.addObjective([], ry.FS.positionRel, [gripper, pushStart], ry.OT.eq, 1e1*np.array([[1,0,0],[0,0,1]]))\n", " #move1->komo->addObjective({}, FS_poseRel, {gripper, obj}, OT_eq, {1e1}, {}, 1); //constant relative pose! (redundant for first switch option)\n", "\n", " M2.solve()\n", " if not M2.ret.feasible:\n", " continue\n", "\n", " M1.play(C, 1.)\n", " C.attach(gripper, obj)\n", " M2.play(C, 1.)\n", " C.attach(table, obj)\n", "\n", "del M" ] }, { "cell_type": "markdown", "id": "064ecdbc", "metadata": {}, "source": [ "## TODOS:\n", "* Proper execution: BotOp instead of display with C\n", "* RRTs\n", "* additional planar motion constraint for in-plane manipulation\n", "* more typical manipulation constraints: camera_look_at, push_straight, " ] }, { "cell_type": "code", "execution_count": null, "id": "3f67eeb3", "metadata": {}, "outputs": [], "source": [ "del C" ] }, { "cell_type": "code", "execution_count": null, "id": "ca604aa1", "metadata": {}, "outputs": [], "source": [] } ], "metadata": { "kernelspec": { "display_name": "venv", "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.12.3" } }, "nbformat": 4, "nbformat_minor": 5 }