Hierarchical Task-Parameterized Learning from Demonstration for Collaborative Object Movement

<table class="algorithm-group"><tr><td><table class="algorithm" id="alg2"><tr><td colspan="2"><b>procedure</b> DISTANCE(<span class="nowrap"><svg height="9.22069pt" id="M106" style="vertical-align:-3.18151pt" version="1.1" viewbox="-0.0498162 -6.03918 14.2298 9.22069" width="14.2298pt" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink"><g transform="matrix(.013,0,0,-0.013,0,0)"><path d="M212 391V464C146 445 80 427 26 418V388C75 381 84 376 84 321V-128C84 -184 80 -192 16 -198V-230H294V-198C216 -191 212 -184 212 -127V0C227 -6 249 -12 274 -12C402 17 517 114 517 261C517 357 454 454 336 454C319 454 301 447 281 435L212 391ZM212 354C229 369 250 378 275 378C324 378 385 333 385 210C385 106 347 38 293 38C268 38 231 59 212 77V354Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,7.111,3.132)"><path d="M722 32C646 41 636 48 622 116L544 496L509 667L472 658L153 123C106 45 93 42 27 32L18 0H259L268 32C178 40 172 48 204 107L272 233H509C523 180 533 137 540 96C548 50 546 39 466 32L457 0H714L722 32ZM502 283H299L448 547H450L502 283Z"></path></g></svg>,</span> <span class="nowrap"><svg height="9.22069pt" id="M107" style="vertical-align:-3.18151pt" version="1.1" viewbox="-0.0498162 -6.03918 13.2889 9.22069" width="13.2889pt" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink"><g transform="matrix(.013,0,0,-0.013,0,0)"><path d="M212 391V464C146 445 80 427 26 418V388C75 381 84 376 84 321V-128C84 -184 80 -192 16 -198V-230H294V-198C216 -191 212 -184 212 -127V0C227 -6 249 -12 274 -12C402 17 517 114 517 261C517 357 454 454 336 454C319 454 301 447 281 435L212 391ZM212 354C229 369 250 378 275 378C324 378 385 333 385 210C385 106 347 38 293 38C268 38 231 59 212 77V354Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,7.111,3.132)"><path d="M598 511C598 627 497 650 401 650H141L131 618C219 611 222 606 210 538L130 119C117 43 108 39 25 32L18 0H236C324 0 396 5 456 30C531 59 585 119 585 193C585 285 520 331 428 351V353C529 373 598 423 598 511ZM502 508C502 418 431 368 330 368H263L298 559C303 584 308 595 317 600C329 608 345 612 369 612C428 612 502 595 502 508ZM483 197C483 95 404 39 309 39C225 39 206 58 219 139L256 330H317C395 330 483 301 483 197Z"></path></g></svg>)</span></td></tr><tr><td colspan="2"> <svg height="9.49473pt" id="M108" style="vertical-align:-0.2063999pt" version="1.1" viewbox="-0.0498162 -9.28833 28.5284 9.49473" width="28.5284pt" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink"><g transform="matrix(.013,0,0,-0.013,0,0)"><path d="M530 686C535 705 530 712 521 712C504 712 448 684 359 674L358 648H393C437 648 439 646 429 593L400 435C372 447 345 448 332 448C286 448 194 414 144 373C68 311 23 203 23 111C23 26 57 -12 91 -12C120 -12 147 3 188 29C227 54 290 102 341 170H343L322 71C308 6 320 -12 341 -12C373 -12 442 27 501 96L485 120C455 91 422 67 408 67C401 67 401 76 404 91C440 294 479 473 530 686ZM387 375L355 241C326 187 200 53 142 53C126 53 109 73 109 130C109 217 154 337 218 381C240 396 265 404 297 404S372 390 387 375Z"></path></g><g transform="matrix(.013,0,0,-0.013,10.847,0)"><path d="M535 323V373H52V323H535ZM535 138V188H52V138H535Z"></path></g><g transform="matrix(.013,0,0,-0.013,22.11,0)"><path d="M241 635C89 635 35 457 35 312C35 153 89 -12 240 -12C390 -12 443 166 443 312C443 466 390 635 241 635ZM238 602C329 602 354 454 354 312C354 172 330 22 240 22C152 22 124 173 124 313S148 602 238 602Z"></path></g></svg></td></tr><tr><td colspan="2"> <b>for</b><svg height="12.0653pt" id="M109" style="vertical-align:-3.4294pt" version="1.1" viewbox="-0.0498162 -8.6359 26.806 12.0653" width="26.806pt" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink"><g transform="matrix(.013,0,0,-0.013,0,0)"><path d="M394 607C394 634 378 655 350 655C315 655 294 620 294 594C294 564 314 546 339 546C372 546 394 574 394 607ZM360 349C373 414 374 448 351 448C321 448 249 404 183 338L197 313C223 336 265 367 276 367C285 367 283 355 275 313C241 129 220 23 189 -102C175 -158 153 -197 124 -197C106 -197 85 -187 70 -177C64 -173 51 -177 44 -183C34 -193 23 -209 23 -228C23 -244 46 -261 67 -261C86 -261 127 -245 181 -197C237 -147 279 -52 303 66L360 349Z"></path></g><g transform="matrix(.013,0,0,-0.013,9.132,0)"><path d="M535 323V373H52V323H535ZM535 138V188H52V138H535Z"></path></g><g transform="matrix(.013,0,0,-0.013,20.395,0)"><path d="M412 140C382 77 369 73 315 73H129L270 222C362 320 402 379 402 466C402 571 322 635 234 635C177 635 130 609 99 576L42 495L64 475C90 514 133 568 201 568C274 568 318 519 318 435C318 349 255 267 193 193C144 135 87 78 32 23V0H405C417 45 427 89 440 131L412 140Z"></path></g></svg> to <svg height="8.55521pt" id="M110" style="vertical-align:-0.2063904pt" version="1.1" viewbox="-0.0498162 -8.34882 6.36303 8.55521" width="6.36303pt" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink"><g transform="matrix(.013,0,0,-0.013,0,0)"><path d="M285 378C315 398 338 416 353 432C373 451 384 474 384 503C384 579 325 635 236 635H235C182 635 136 610 108 579L65 516L85 496C110 533 150 575 205 575C258 575 300 543 300 481C300 407 232 369 141 339L147 310C163 315 188 321 211 321C268 321 338 284 338 192C338 94 288 40 217 40C160 40 119 68 93 91C85 98 77 97 69 91C60 84 47 71 46 58C44 46 48 35 62 22C75 10 116 -12 162 -12C234 -12 424 62 424 224C424 297 373 359 285 376V378Z"></path></g></svg><b>do</b></td></tr><tr><td colspan="2">  <svg height="16.3896pt" id="M111" style="vertical-align:-5.528901pt" version="1.1" viewbox="-0.0498162 -10.8607 228.23 16.3896" width="228.23pt" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink"><g transform="matrix(.013,0,0,-0.013,0,0)"><path d="M530 686C535 705 530 712 521 712C504 712 448 684 359 674L358 648H393C437 648 439 646 429 593L400 435C372 447 345 448 332 448C286 448 194 414 144 373C68 311 23 203 23 111C23 26 57 -12 91 -12C120 -12 147 3 188 29C227 54 290 102 341 170H343L322 71C308 6 320 -12 341 -12C373 -12 442 27 501 96L485 120C455 91 422 67 408 67C401 67 401 76 404 91C440 294 479 473 530 686ZM387 375L355 241C326 187 200 53 142 53C126 53 109 73 109 130C109 217 154 337 218 381C240 396 265 404 297 404S372 390 387 375Z"></path></g><g transform="matrix(.013,0,0,-0.013,10.847,0)"><path d="M535 323V373H52V323H535ZM535 138V188H52V138H535Z"></path></g><g transform="matrix(.013,0,0,-0.013,22.11,0)"><path d="M530 686C535 705 530 712 521 712C504 712 448 684 359 674L358 648H393C437 648 439 646 429 593L400 435C372 447 345 448 332 448C286 448 194 414 144 373C68 311 23 203 23 111C23 26 57 -12 91 -12C120 -12 147 3 188 29C227 54 290 102 341 170H343L322 71C308 6 320 -12 341 -12C373 -12 442 27 501 96L485 120C455 91 422 67 408 67C401 67 401 76 404 91C440 294 479 473 530 686ZM387 375L355 241C326 187 200 53 142 53C126 53 109 73 109 130C109 217 154 337 218 381C240 396 265 404 297 404S372 390 387 375Z"></path></g><g transform="matrix(.013,0,0,-0.013,32.231,0)"><path d="M535 230V280H323V490H265V280H52V230H265V-3H323V230H535Z"></path></g><g transform="matrix(.013,0,0,-0.013,42.767,0)"><path d="M162 -163V703H102V-163H162ZM309 -163V703H249V-163H309Z"></path></g><g transform="matrix(.013,0,0,-0.013,48.11,0)"><path d="M660 23C638 31 621 43 599 66C580 87 558 119 523 175C493 221 456 287 441 317C508 344 566 392 566 480C566 542 539 586 499 613C458 640 408 650 320 650H39V615C112 608 121 598 121 518V134C121 51 110 42 35 35V0H337V35C261 42 252 51 252 134V280H271C303 280 321 267 337 237C366 184 393 130 419 89C460 26 498 -2 624 -8C637 -8 648 -9 654 -9L660 23ZM252 320V563C252 584 254 594 261 600C267 606 281 610 306 610C362 610 424 572 424 465C424 414 410 374 376 346C354 327 320 320 278 320H252Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,56.69,-5.741)"><path d="M645 502V555H55V502H319V0H381V502H645Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,56.677,3.784)"><path d="M389 0V32C297 38 291 46 291 118V635C234 613 175 595 109 583V556L161 554C203 552 207 547 207 497V118C207 46 201 38 110 32V0H389Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,61.109,3.784)"><path d="M98 134C72 134 46 117 46 90C46 73 55 65 60 64C95 55 124 32 124 -4C124 -42 95 -68 44 -89L57 -123C122 -104 194 -60 194 22C194 94 136 134 98 134Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,63.266,3.784)"><path d="M722 32C646 41 636 48 622 116L544 496L509 667L472 658L153 123C106 45 93 42 27 32L18 0H259L268 32C178 40 172 48 204 107L272 233H509C523 180 533 137 540 96C548 50 546 39 466 32L457 0H714L722 32ZM502 283H299L448 547H450L502 283Z"></path></g><g transform="matrix(.013,0,0,-0.013,70.283,0)"><path d="M300 -147C201 -63 143 98 143 270S200 602 300 686L282 710C136 610 70 450 70 271V270C70 89 136 -72 282 -170L300 -147Z"></path></g><g transform="matrix(.013,0,0,-0.013,74.781,0)"><path d="M220 349V455C158 436 98 425 38 417V389C85 378 92 374 92 323V104C92 43 83 38 28 32V0H302V32C225 38 220 43 220 104V291C237 330 257 343 276 343C292 343 305 336 322 322C329 316 339 313 353 318C379 328 402 356 402 392C402 426 374 454 335 454C297 454 259 411 222 349H220Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,80.033,3.132)"><path d="M400 606C400 634 383 656 353 656C316 656 294 620 294 593C294 564 317 545 343 545C375 545 400 573 400 606ZM366 351C379 413 381 451 356 451C323 451 251 408 183 341L199 313C223 335 267 365 277 365C285 365 284 354 277 312C245 132 222 27 193 -100C182 -148 160 -188 131 -188C113 -188 90 -178 75 -170C64 -164 55 -168 48 -175C38 -185 24 -203 24 -222S48 -257 71 -257C89 -257 131 -241 186 -192C243 -141 286 -46 310 74L366 351Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,83.947,3.132)"><path d="M98 134C72 134 46 117 46 90C46 73 55 65 60 64C95 55 124 32 124 -4C124 -42 95 -68 44 -89L57 -123C122 -104 194 -60 194 22C194 94 136 134 98 134Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,86.103,3.132)"><path d="M722 32C646 41 636 48 622 116L544 496L509 667L472 658L153 123C106 45 93 42 27 32L18 0H259L268 32C178 40 172 48 204 107L272 233H509C523 180 533 137 540 96C548 50 546 39 466 32L457 0H714L722 32ZM502 283H299L448 547H450L502 283Z"></path></g><g transform="matrix(.013,0,0,-0.013,96.023,0)"><path d="M535 230V280H52V230H535Z"></path></g><g transform="matrix(.013,0,0,-0.013,106.56,0)"><path d="M220 349V455C158 436 98 425 38 417V389C85 378 92 374 92 323V104C92 43 83 38 28 32V0H302V32C225 38 220 43 220 104V291C237 330 257 343 276 343C292 343 305 336 322 322C329 316 339 313 353 318C379 328 402 356 402 392C402 426 374 454 335 454C297 454 259 411 222 349H220Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,111.812,3.132)"><path d="M389 0V32C297 38 291 46 291 118V635C234 613 175 595 109 583V556L161 554C203 552 207 547 207 497V118C207 46 201 38 110 32V0H389Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,116.243,3.132)"><path d="M98 134C72 134 46 117 46 90C46 73 55 65 60 64C95 55 124 32 124 -4C124 -42 95 -68 44 -89L57 -123C122 -104 194 -60 194 22C194 94 136 134 98 134Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,118.4,3.132)"><path d="M722 32C646 41 636 48 622 116L544 496L509 667L472 658L153 123C106 45 93 42 27 32L18 0H259L268 32C178 40 172 48 204 107L272 233H509C523 180 533 137 540 96C548 50 546 39 466 32L457 0H714L722 32ZM502 283H299L448 547H450L502 283Z"></path></g><g transform="matrix(.013,0,0,-0.013,125.417,0)"><path d="M275 270C275 450 212 609 64 710L45 686C145 604 203 442 203 270S147 -63 45 -147L64 -170C213 -68 275 89 275 270Z"></path></g><g transform="matrix(.013,0,0,-0.013,132.821,0)"><path d="M535 230V280H52V230H535Z"></path></g><g transform="matrix(.013,0,0,-0.013,143.357,0)"><path d="M660 23C638 31 621 43 599 66C580 87 558 119 523 175C493 221 456 287 441 317C508 344 566 392 566 480C566 542 539 586 499 613C458 640 408 650 320 650H39V615C112 608 121 598 121 518V134C121 51 110 42 35 35V0H337V35C261 42 252 51 252 134V280H271C303 280 321 267 337 237C366 184 393 130 419 89C460 26 498 -2 624 -8C637 -8 648 -9 654 -9L660 23ZM252 320V563C252 584 254 594 261 600C267 606 281 610 306 610C362 610 424 572 424 465C424 414 410 374 376 346C354 327 320 320 278 320H252Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,151.937,-5.741)"><path d="M645 502V555H55V502H319V0H381V502H645Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,151.924,3.784)"><path d="M389 0V32C297 38 291 46 291 118V635C234 613 175 595 109 583V556L161 554C203 552 207 547 207 497V118C207 46 201 38 110 32V0H389Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,156.356,3.784)"><path d="M98 134C72 134 46 117 46 90C46 73 55 65 60 64C95 55 124 32 124 -4C124 -42 95 -68 44 -89L57 -123C122 -104 194 -60 194 22C194 94 136 134 98 134Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,158.513,3.784)"><path d="M598 511C598 627 497 650 401 650H141L131 618C219 611 222 606 210 538L130 119C117 43 108 39 25 32L18 0H236C324 0 396 5 456 30C531 59 585 119 585 193C585 285 520 331 428 351V353C529 373 598 423 598 511ZM502 508C502 418 431 368 330 368H263L298 559C303 584 308 595 317 600C329 608 345 612 369 612C428 612 502 595 502 508ZM483 197C483 95 404 39 309 39C225 39 206 58 219 139L256 330H317C395 330 483 301 483 197Z"></path></g><g transform="matrix(.013,0,0,-0.013,164.589,0)"><path d="M300 -147C201 -63 143 98 143 270S200 602 300 686L282 710C136 610 70 450 70 271V270C70 89 136 -72 282 -170L300 -147Z"></path></g><g transform="matrix(.013,0,0,-0.013,169.087,0)"><path d="M220 349V455C158 436 98 425 38 417V389C85 378 92 374 92 323V104C92 43 83 38 28 32V0H302V32C225 38 220 43 220 104V291C237 330 257 343 276 343C292 343 305 336 322 322C329 316 339 313 353 318C379 328 402 356 402 392C402 426 374 454 335 454C297 454 259 411 222 349H220Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,174.339,3.132)"><path d="M400 606C400 634 383 656 353 656C316 656 294 620 294 593C294 564 317 545 343 545C375 545 400 573 400 606ZM366 351C379 413 381 451 356 451C323 451 251 408 183 341L199 313C223 335 267 365 277 365C285 365 284 354 277 312C245 132 222 27 193 -100C182 -148 160 -188 131 -188C113 -188 90 -178 75 -170C64 -164 55 -168 48 -175C38 -185 24 -203 24 -222S48 -257 71 -257C89 -257 131 -241 186 -192C243 -141 286 -46 310 74L366 351Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,178.253,3.132)"><path d="M98 134C72 134 46 117 46 90C46 73 55 65 60 64C95 55 124 32 124 -4C124 -42 95 -68 44 -89L57 -123C122 -104 194 -60 194 22C194 94 136 134 98 134Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,180.409,3.132)"><path d="M598 511C598 627 497 650 401 650H141L131 618C219 611 222 606 210 538L130 119C117 43 108 39 25 32L18 0H236C324 0 396 5 456 30C531 59 585 119 585 193C585 285 520 331 428 351V353C529 373 598 423 598 511ZM502 508C502 418 431 368 330 368H263L298 559C303 584 308 595 317 600C329 608 345 612 369 612C428 612 502 595 502 508ZM483 197C483 95 404 39 309 39C225 39 206 58 219 139L256 330H317C395 330 483 301 483 197Z"></path></g><g transform="matrix(.013,0,0,-0.013,189.388,0)"><path d="M535 230V280H52V230H535Z"></path></g><g transform="matrix(.013,0,0,-0.013,199.925,0)"><path d="M220 349V455C158 436 98 425 38 417V389C85 378 92 374 92 323V104C92 43 83 38 28 32V0H302V32C225 38 220 43 220 104V291C237 330 257 343 276 343C292 343 305 336 322 322C329 316 339 313 353 318C379 328 402 356 402 392C402 426 374 454 335 454C297 454 259 411 222 349H220Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,205.177,3.132)"><path d="M389 0V32C297 38 291 46 291 118V635C234 613 175 595 109 583V556L161 554C203 552 207 547 207 497V118C207 46 201 38 110 32V0H389Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,209.609,3.132)"><path d="M98 134C72 134 46 117 46 90C46 73 55 65 60 64C95 55 124 32 124 -4C124 -42 95 -68 44 -89L57 -123C122 -104 194 -60 194 22C194 94 136 134 98 134Z"></path></g><g transform="matrix(.0091,0,0,-0.0091,211.765,3.132)"><path d="M598 511C598 627 497 650 401 650H141L131 618C219 611 222 606 210 538L130 119C117 43 108 39 25 32L18 0H236C324 0 396 5 456 30C531 59 585 119 585 193C585 285 520 331 428 351V353C529 373 598 423 598 511ZM502 508C502 418 431 368 330 368H263L298 559C303 584 308 595 317 600C329 608 345 612 369 612C428 612 502 595 502 508ZM483 197C483 95 404 39 309 39C225 39 206 58 219 139L256 330H317C395 330 483 301 483 197Z"></path></g><g transform="matrix(.013,0,0,-0.013,217.842,0)"><path d="M275 270C275 450 212 609 64 710L45 686C145 604 203 442 203 270S147 -63 45 -147L64 -170C213 -68 275 89 275 270Z"></path></g><g transform="matrix(.013,0,0,-0.013,222.34,0)"><path d="M162 -163V703H102V-163H162ZM309 -163V703H249V-163H309Z"></path></g></svg></td></tr><tr><td colspan="2"> <b>return</b><svg height="9.49473pt" id="M112" style="vertical-align:-0.2063999pt" version="1.1" viewbox="-0.0498162 -9.28833 7.34169 9.49473" width="7.34169pt" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink"><g transform="matrix(.013,0,0,-0.013,0,0)"><path d="M530 686C535 705 530 712 521 712C504 712 448 684 359 674L358 648H393C437 648 439 646 429 593L400 435C372 447 345 448 332 448C286 448 194 414 144 373C68 311 23 203 23 111C23 26 57 -12 91 -12C120 -12 147 3 188 29C227 54 290 102 341 170H343L322 71C308 6 320 -12 341 -12C373 -12 442 27 501 96L485 120C455 91 422 67 408 67C401 67 401 76 404 91C440 294 479 473 530 686ZM387 375L355 241C326 187 200 53 142 53C126 53 109 73 109 130C109 217 154 337 218 381C240 396 265 404 297 404S372 390 387 375Z"></path></g></svg></td></tr></table></td></tr></table>

Applied Bionics and Biomechanics

alg2

Algorithm 2

Algorithm 2: Hierarchical Task-Parameterized Learning from Demonstration for Collaborative Object Movement 