2_geometry_3d.ipynb 22.5 KB
 Nicolas Mansard committed Jan 20, 2021 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 ``````{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "# Direct and inverse geometry of 3d robots\n", "This notebook introduces the kinematic tree of Pinocchio for a serial manipulator, explain how to compute the forward and inverse geometry (from configuration to end-effector placements, and inversely). The ideas are examplified with a simplified case-study taken from parallel robotics." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ `````` Guilhem Saurel committed Aug 09, 2021 17 `````` "import magic_donotload # noqa: F401" `````` Nicolas Mansard committed Jan 20, 2021 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 `````` ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Set up\n", "We will need Pinocchio, Gepetto-Viewer, SciPy for the solvers" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import time\n", "import numpy as np\n", `````` Guilhem Saurel committed Aug 09, 2021 36 `````` "from numpy.linalg import norm\n", `````` Nicolas Mansard committed Jan 20, 2021 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 `````` "import pinocchio as pin\n", "import example_robot_data as robex\n", "from scipy.optimize import fmin_bfgs" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Kinematic tree in Pinocchio\n", "Let's now play with 3D robots. We will load the models from URDF files.\n", "\n", "*The robot UR5* is a low-cost manipulator robot with good performances. It is a fixed robot with one 6-DOF arms developed by the Danish company Universal Robot. All its 6 joints are revolute joints. Its configuration is in R^6 and is not subject to any constraint. The model of UR5 is described in a URDF file, with the visuals of the bodies of the robot being described as meshed (i.e. polygon soups) using the Collada format \".dae\". Both the URDF and the DAE files are available in the repository in the model directory. \n", "\n", "This robot model, as well as other models used in the notebooks, are installed from the apt paquet robotpkg-example-robot-data and stored in /opt/openrobots/share/example-robot-data.\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ `````` Guilhem Saurel committed Aug 09, 2021 60 `````` "# %load tp2/generated/simple_pick_and_place_1\n", `````` Guilhem Saurel committed Aug 09, 2021 61 `````` "\n", `````` Nicolas Mansard committed Jan 20, 2021 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 `````` "robot = robex.load('ur5')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The kinematic tree is represented by two C++ objects called Model (which contains the model constants: lengths, masses, names, etc) and Data (which contains the working memory used by the model algorithms). Both C\\++ objects are contained in a unique Python class. The first class is called RobotWrapper and is generic." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "print(robot.model)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "For the next steps, we are going to work with the RobotWrapper.\n", "\n", "Import the class RobotWrapper and create an instance of this class in the python terminal. At initialization, RobotWrapper will read the model description in the URDF file given as argument. In the following, we will use the model of the UR5 robot, available in the directory \"models\" of pinocchio (available in the homedir of the VBox). The code of the RobotWrapper class is in /opt/openrobots/lib/python2.7/site-packages/pinocchio/robot_wrapper.py . Do not hesitate to have a look at it and to take inspiration from the implementation of the class functions.\n", "\n", "Here are some import methods of the class.\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "* robot.q0 contains a reference initial configuration of the robot (not a pretty good one for the UR-5)." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "* robot.index('joint name') returns the index of the joint." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "robot.index(' wrist_3_joint')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "* robot.model.names is a container (~list) that contains all the joint names" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ `````` Guilhem Saurel committed Aug 09, 2021 128 129 `````` "for i, n in enumerate(robot.model.names):\n", " print(i, n)" `````` Nicolas Mansard committed Jan 20, 2021 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 `````` ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "* robot.model.frames contains all the import frames attached to the robot. " ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "for f in robot.model.frames:\n", `````` Guilhem Saurel committed Aug 09, 2021 146 `````` " print(f.name, 'attached to joint #', f.parent)" `````` Nicolas Mansard committed Jan 20, 2021 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 `````` ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "* robot.placement(idx) and robot.framePlacement(idx) returns the placement (i.e. translation+rotation of the joint / frame in argument." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ `````` Guilhem Saurel committed Aug 09, 2021 162 `````` "robot.placement(robot.q0, 6) # Placement of the end effector." `````` Nicolas Mansard committed Jan 20, 2021 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 `````` ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The dimension of the configuration space (i.e. the number of joints) is given in:" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "NQ = robot.model.nq\n", `````` Guilhem Saurel committed Aug 09, 2021 179 `````` "NV = robot.model.nv # for this simple robot, NV == NQ" `````` Nicolas Mansard committed Jan 20, 2021 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 `````` ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Display simple geometries\n", "The robot is displayed in the viewer. We are going to use Meshcat to visualize the 3d robot and scene. First open the viewer and load the robot geometries." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ `````` Guilhem Saurel committed Aug 09, 2021 196 197 `````` "from utils.meshcat_viewer_wrapper import MeshcatVisualizer, colors # noqa: E402\n", "\n", `````` Nicolas Mansard committed Jan 20, 2021 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 `````` "viz = MeshcatVisualizer(robot)" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "viz.viewer.jupyter_cell()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "A configuration *q* can be displayed in the viewer:" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "q = np.array([-1., -1.5, 2.1, -.5, -.5, 0])\n", "\n", `````` Guilhem Saurel committed Aug 09, 2021 225 `````` "viz.display(q)" `````` Nicolas Mansard committed Jan 20, 2021 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 `````` ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Other geometries (cubes, spheres, etc) can be displayed as well." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ `````` Guilhem Saurel committed Aug 09, 2021 241 `````` "# %load tp2/generated/simple_pick_and_place_2\n", `````` Nicolas Mansard committed Jan 20, 2021 242 243 244 `````` "\n", "# Add a red box in the viewer\n", "ballID = \"world/ball\"\n", `````` Guilhem Saurel committed Aug 09, 2021 245 `````` "viz.addSphere(ballID, 0.1, colors.red)\n", `````` Nicolas Mansard committed Jan 20, 2021 246 247 248 249 `````` "\n", "# Place the ball at the position ( 0.5, 0.1, 0.2 )\n", "# The viewer expect position and rotation, apppend the identity quaternion\n", "q_ball = [0.5, 0.1, 0.2, 1, 0, 0, 0]\n", `````` Guilhem Saurel committed Aug 09, 2021 250 `````` "viz.applyConfiguration(ballID, q_ball)" `````` Nicolas Mansard committed Jan 20, 2021 251 252 253 254 255 256 `````` ] }, { "cell_type": "markdown", "metadata": {}, "source": [ `````` Guilhem Saurel committed Aug 09, 2021 257 `````` "# Forward (direct) geometry\n", `````` Nicolas Mansard committed Jan 20, 2021 258 259 260 `````` "\n", "First, let's do some forward geometry, i.e. use Pinocchio to compute where is the end effector knowning the robot configuration.\n", "\n", `````` Guilhem Saurel committed Aug 09, 2021 261 `````` "# Simple pick ...\n", `````` Nicolas Mansard committed Jan 20, 2021 262 263 264 265 266 267 268 269 270 271 272 `````` "\n", "Say we have a target at position [.5,.1,.2] and we would like the robot to grasp it.\n", "First decide (by any way you want, e.g. trial and error) the configuration of the robot so that the end effector touches the ball.\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ `````` Guilhem Saurel committed Aug 09, 2021 273 `````` "# %load tp2/generated/simple_pick_and_place_3\n", `````` Nicolas Mansard committed Jan 20, 2021 274 `````` "\n", `````` Guilhem Saurel committed Aug 09, 2021 275 `````` "q0 = np.zeros(NQ) # set the correct values here\n", `````` Nicolas Mansard committed Jan 20, 2021 276 277 278 279 280 281 `````` "q0[0] = 0.5\n", "q0[1] = 0.\n", "q0[2] = -1.5\n", "q0[3] = 0.\n", "q0[4] = 0.\n", "q0[5] = 0.\n", `````` Guilhem Saurel committed Aug 09, 2021 282 `````` "\n", `````` Nicolas Mansard committed Jan 20, 2021 283 284 285 286 287 288 289 290 291 292 `````` "viz.display(q0)\n", "\n", "# Take care to explicitely mention copy when you want a copy of array.\n", "q = q0.copy()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ `````` Guilhem Saurel committed Aug 09, 2021 293 `````` "# ... and simple place" `````` Nicolas Mansard committed Jan 20, 2021 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 `````` ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "At the reference position you built, the end effector placement can be obtained by calling " ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ `````` Guilhem Saurel committed Aug 09, 2021 309 `````` "robot.placement(q, 6).translation" `````` Nicolas Mansard committed Jan 20, 2021 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 `````` ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Only the translation part of the placement has been selected. The rotation is free.\n", "\n", "Now, choose any trajectory you want in the configuration space (it can be sinus-cosinus waves, polynomials, splines, straight lines). Make a for loop to display the robot at sampling positions along this trajectory. The function sleep can be used to slow down the loop." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ `````` Guilhem Saurel committed Aug 09, 2021 327 `````` "time.sleep(.1) # in second" `````` Nicolas Mansard committed Jan 20, 2021 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 `````` ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "At each instant of your loop, recompute the position of the ball and display it so that it always \"sticks\" to the robot end effector." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ `````` Guilhem Saurel committed Aug 09, 2021 343 344 345 346 `````` "# TODO ####################################################\n", "# Replace here with your initial configuration\n", "q = q0 = np.random.rand(NQ) * 6 - 3\n", "# TODO ####################################################\n", `````` Nicolas Mansard committed Jan 20, 2021 347 348 349 350 351 352 353 354 355 `````` "\n", "# Compute initial translation between effector and box.\n", "# Translation of end-eff wrt world at initial configuration\n", "o_eff = robot.placement(q, 6).translation\n", "# Translation of box wrt world\n", "o_ball = q_ball[:3]\n", "eff_ball = o_ball - o_eff\n", "\n", "for i in range(100):\n", `````` Guilhem Saurel committed Aug 09, 2021 356 `````` " # Replace here by your choice of computing q(t)\n", `````` Guilhem Saurel committed Aug 09, 2021 357 `````` " q += np.random.rand(6) * 2e-1 - 1e-1\n", `````` Nicolas Mansard committed Jan 20, 2021 358 `````` "\n", `````` Guilhem Saurel committed Aug 09, 2021 359 360 `````` " # TODO ####################################################\n", " # Replace here by your computation of the new box position\n", `````` Guilhem Saurel committed Aug 09, 2021 361 `````` " o_ball = np.array([0., 0., 0.])\n", `````` Guilhem Saurel committed Aug 09, 2021 362 363 `````` " # /TODO ###################################################\n", "\n", `````` Nicolas Mansard committed Jan 20, 2021 364 365 `````` " # Display the new robot and box configurations.\n", " # The viewer expect a placement (position-rotation).\n", `````` Guilhem Saurel committed Aug 09, 2021 366 `````` " viz.applyConfiguration(ballID, o_ball.tolist() + [1, 0, 0, 0])\n", `````` Nicolas Mansard committed Jan 20, 2021 367 `````` " viz.display(q)\n", `````` Guilhem Saurel committed Aug 09, 2021 368 `````` " time.sleep(0.1)" `````` Nicolas Mansard committed Jan 20, 2021 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 `````` ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The solution is below, should you need it." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ `````` Guilhem Saurel committed Aug 09, 2021 384 `````` "%do_not_load tp2/generated/simple_pick_and_place_4" `````` Nicolas Mansard committed Jan 20, 2021 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 `````` ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Pick and place in 3D\n", "\n", "Say now that the object is a rectangle and not a sphere. Pick the object at a reference position with the rotation that is imposed, so that the end effector is aligned with one of the faces of the rectangle.\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ `````` Guilhem Saurel committed Aug 09, 2021 402 `````` "# %load tp2/generated/simple_pick_and_place_5\n", `````` Guilhem Saurel committed Aug 09, 2021 403 `````` "\n", `````` Nicolas Mansard committed Jan 20, 2021 404 405 `````` "# Add a red box in the viewer\n", "boxID = \"world/box\"\n", `````` Guilhem Saurel committed Aug 09, 2021 406 407 `````` "viz.delete(ballID)\n", "viz.addBox(boxID, [0.1, 0.2, 0.1], colors.magenta)\n", `````` Nicolas Mansard committed Jan 20, 2021 408 `````` "\n", `````` Guilhem Saurel committed Aug 09, 2021 409 `````` "# Place the box at the position (0.5, 0.1, 0.2)\n", `````` Nicolas Mansard committed Jan 20, 2021 410 `````` "q_box = [0.5, 0.1, 0.2, 1, 0, 0, 0]\n", `````` Guilhem Saurel committed Aug 09, 2021 411 `````` "viz.applyConfiguration('world/box', q_box)" `````` Nicolas Mansard committed Jan 20, 2021 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 `````` ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "A configuration with the arm nicely attached to the box is:" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ `````` Guilhem Saurel committed Aug 09, 2021 427 `````` "# %load tp2/generated/simple_pick_and_place_6\n", `````` Guilhem Saurel committed Aug 09, 2021 428 `````` "\n", `````` Nicolas Mansard committed Jan 20, 2021 429 430 431 432 433 `````` "q0 = np.zeros(NQ)\n", "q0[0] = -0.375\n", "q0[1] = -1.2\n", "q0[2] = 1.71\n", "q0[3] = -q0[1] - q0[2]\n", `````` Guilhem Saurel committed Aug 09, 2021 434 `````` "q0[4] = q0[0]\n", `````` Nicolas Mansard committed Jan 20, 2021 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 `````` "\n", "viz.display(q0)\n", "q = q0.copy()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Redo the same question as before, but now also choosing the orientation of the box. For that, at each robot configuration in your for-loop, compute the box placement wrt the world (let's denote it by oMbox) and display both the box and the robot configuration in the view." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ `````` Guilhem Saurel committed Aug 09, 2021 453 `````` "%do_not_load tp2/generated/simple_pick_and_place_7" `````` Nicolas Mansard committed Jan 20, 2021 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 `````` ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Inverse geometry\n", "\n", "We only yet computed the forward geometry, i.e. from configurations to end-effector placement. Let's to the inverse map not.\n", "\n", "### Inverse geometry in 3D\n", "\n", "Let's now first control the position (i.e. translation only) of the end effector of a manipulator robot to a given position. For this first part, we will use the fixed serial-chain robot model.\n", "\n", "Recall first that the position (3D) of the joint with index \"i=6\" at position \"q\" can be access by the following two lines of code." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ `````` Guilhem Saurel committed Aug 09, 2021 477 `````` "robot.placement(q, 6).translation" `````` Nicolas Mansard committed Jan 20, 2021 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 `````` ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Using the scipy solver [used in the previous notebook](1_geometry_2d.ipynb#section_optim), compute a configuration q where the end effector reaches p. For that, implement a cost function that takes a configuration as argument and returns the squared distance between the end effetor and the target." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ `````` Guilhem Saurel committed Aug 09, 2021 493 `````` "%do_not_load tp2/generated/invgeom3d_1" `````` Nicolas Mansard committed Jan 20, 2021 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 `````` ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Inverse geometry in 6D\n", "6D means: translation and rotation. Change the previous cost function for a cost measuring the difference between the current placement root.placement(q,6) and a reference placement oMdes. \n", "For that, you can use the SE(3) log function to score the distance between two placements. The log returns a 6D velocity, represented by a class Motion, that must be transformed to a vector of R^6 from which you can take the norm.\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "pin.log(pin.SE3.Identity()).vector" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ `````` Guilhem Saurel committed Aug 09, 2021 520 `````` "# %load tp2/generated/invgeom6d_1\n", `````` Guilhem Saurel committed Aug 09, 2021 521 522 523 524 `````` "\n", "# --- Add box to represent target\n", "viz.addBox(\"world/box\", [.05, .1, .2], [1., .2, .2, .5])\n", "viz.addBox(\"world/blue\", [.05, .1, .2], [.2, .2, 1., .5])\n", `````` Nicolas Mansard committed Jan 20, 2021 525 526 527 528 529 `````` "\n", "#\n", "# OPTIM 6D #########################################################\n", "#\n", "\n", `````` Guilhem Saurel committed Aug 09, 2021 530 `````` "\n", `````` Nicolas Mansard committed Jan 20, 2021 531 532 533 534 535 `````` "def cost(q):\n", " '''Compute score from a configuration'''\n", " M = robot.placement(q, 6)\n", " return norm(pin.log(M.inverse() * Mtarget).vector)\n", "\n", `````` Guilhem Saurel committed Aug 09, 2021 536 `````` "\n", `````` Nicolas Mansard committed Jan 20, 2021 537 `````` "def callback(q):\n", `````` Guilhem Saurel committed Aug 09, 2021 538 `````` " viz.applyConfiguration('world/box', Mtarget)\n", `````` Nicolas Mansard committed Jan 20, 2021 539 540 541 542 `````` " viz.applyConfiguration('world/blue', robot.placement(q, 6))\n", " viz.display(q)\n", " time.sleep(1e-1)\n", "\n", `````` Guilhem Saurel committed Aug 09, 2021 543 `````` "\n", `````` Guilhem Saurel committed Aug 09, 2021 544 `````` "Mtarget = pin.SE3(pin.utils.rotate('x', 3.14 / 4), np.array([0.5, 0.1, 0.2])) # x,y,z\n", `````` Nicolas Mansard committed Jan 20, 2021 545 546 `````` "qopt = fmin_bfgs(cost, robot.q0, callback=callback)\n", "\n", `````` Guilhem Saurel committed Aug 09, 2021 547 `````` "print('The robot finally reached effector placement at\\n', robot.placement(qopt, 6))" `````` Nicolas Mansard committed Jan 20, 2021 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 `````` ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Optimizing in the quaternion space\n", "\n", "Let's now work with a floating robot: the quadruped ANYmal. This robot has 12 joints, but Q-space of size 19 (robot.model.nq) and Q-tangent space of size 18 (robot.model.nv). This is because with need 7D vector to encode the robot placement in space, which indeed to only 6 DOF.\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "robot = robex.load('solo12')\n", "viz = MeshcatVisualizer(robot)\n", "viz.viewer.jupyter_cell()" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "viz.display(robot.q0)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "\n", "Run the following code. Can you explain what just happened? Then correct it to have a proper optimization of ANYmal configuration." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ `````` Guilhem Saurel committed Aug 09, 2021 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 `````` "# %load tp2/generated/floating_1\n", "\n", "robot.feetIndexes = [robot.model.getFrameId(frameName) for frameName in ['HR_FOOT', 'HL_FOOT', 'FR_FOOT', 'FL_FOOT']]\n", "\n", "# --- Add box to represent target\n", "colors = ['red', 'blue', 'green', 'magenta']\n", "for color in colors:\n", " viz.addSphere(\"world/%s\" % color, .05, color)\n", " viz.addSphere(\"world/%s_des\" % color, .05, color)\n", "\n", "#\n", "# OPTIM 6D #########################################################\n", "#\n", "\n", "targets = [\n", " np.array([-0.7, -0.2, 1.2]),\n", " np.array([-0.3, 0.5, 0.8]),\n", " np.array([0.3, 0.1, -0.1]),\n", " np.array([0.9, 0.9, 0.5])\n", "]\n", "for i in range(4):\n", " targets[i][2] += 1\n", "\n", "\n", "def cost(q):\n", " '''Compute score from a configuration'''\n", " cost = 0.\n", " for i in range(4):\n", " p_i = robot.framePlacement(q, robot.feetIndexes[i]).translation\n", " cost += norm(p_i - targets[i])**2\n", " return cost\n", "\n", "\n", "def callback(q):\n", " viz.applyConfiguration('world/box', Mtarget)\n", "\n", " for i in range(4):\n", " p_i = robot.framePlacement(q, robot.feetIndexes[i])\n", " viz.applyConfiguration('world/%s' % colors[i], p_i)\n", " viz.applyConfiguration('world/%s_des' % colors[i], list(targets[i]) + [1, 0, 0, 0])\n", "\n", " viz.display(q)\n", " time.sleep(1e-1)\n", "\n", "\n", "Mtarget = pin.SE3(pin.utils.rotate('x', 3.14 / 4), np.array([0.5, 0.1, 0.2])) # x,y,z\n", "qopt = fmin_bfgs(cost, robot.q0, callback=callback)" `````` Nicolas Mansard committed Jan 20, 2021 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 `````` ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Configuration of parallel robots\n", "A parallel robot is composed of several kinematic chains (called the robot legs) that are all attached to the same end effector. This imposes strict constraints in the configuration space of the robot: a configuration is valide iff all the legs meets the same end-effector placement. We consider here only the geometry aspect of parallel robots (additionnally, some joints are not actuated, which causes additional problems).\n", "\n", "The kinematic structure of a paralel robot indeed induces loops in the joint connection graph. In Pinocchio, we can only represents (one of) the underlying kinematic tree. The loop constraints have to be handled separately. An example that loads 4 manipulator arms is given below." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ `````` Guilhem Saurel committed Aug 09, 2021 658 659 `````` "import utils.load_ur5_parallel as robex2 # noqa: E402\n", "\n", `````` Nicolas Mansard committed Jan 20, 2021 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 `````` "robot = robex2.load()" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "scrolled": true }, "outputs": [], "source": [ "viz = MeshcatVisualizer(robot)\n", "viz.viewer.jupyter_cell()" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "viz.display(robot.q0)" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "[w, h, d] = [0.5, 0.5, 0.005]\n", "color = [red, green, blue, transparency] = [1, 1, 0.78, .8]\n", "viz.addBox('world/robot0/toolplate', [w, h, d], color)\n", "Mtool = pin.SE3(pin.utils.rotate('z', 1.268), np.array([0, 0, .75]))\n", "viz.applyConfiguration('world/robot0/toolplate', Mtool)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The 4 legs of the robot are loaded in a single robot model. The 4 effector placements are computed by:" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ `````` Guilhem Saurel committed Aug 09, 2021 710 711 `````` "effIdxs = [robot.model.getFrameId('tool0_#%d' % i) for i in range(4)]\n", "robot.framePlacement(robot.q0, effIdxs[0])" `````` Nicolas Mansard committed Jan 20, 2021 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 `````` ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The loop constraints are that the relative placement of every leg end-effector must stay the same that in the initial configuration given as example in with the configuration *robot.q0* and the plate placement *Mtool*. To be valid, a configuration *q* must satisfy these 4 relative placement constraints.\n", "\n", "Consider now that the orientation of the tool plate is given by the following quaternion, with the translation that you like (see [the notebook about rotations if you need more details](appendix1_quaternions.ipynb)): \n", "\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ `````` Guilhem Saurel committed Aug 09, 2021 730 `````` "quat = pin.Quaternion(0.7, 0.2, 0.2, 0.6).normalized()\n", `````` Nicolas Mansard committed Jan 20, 2021 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 `````` "print(quat.matrix())" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "**Find using the above optimization routines the configuration of each robot leg so that the loop constraints are all met** for the new orientation of the plate." ] } ], "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", `````` Guilhem Saurel committed Aug 09, 2021 758 `````` "version": "3.9.6" `````` Nicolas Mansard committed Jan 20, 2021 759 760 761 762 `````` } }, "nbformat": 4, "nbformat_minor": 2 `````` Guilhem Saurel committed Aug 09, 2021 763 ``}``