├── LICENSE ├── README.md ├── animation.py ├── constant.py ├── mathematics.py ├── media ├── ik-issue.gif ├── leg_pose1.png ├── leg_pose2.png ├── plt_ani_tripod_gait.gif ├── plt_body_ik.gif ├── plt_ex1_ik.png ├── plt_ex1_initial.png ├── useage-dimension.gif ├── useage-fk.gif ├── useage-gait.gif └── useage-ik.gif ├── models.py ├── style.ini └── ui.py /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Hexapod Robot Simulator 2 | 3 | A simulator for hexapod **pose** and **gait** control based on *forward* and *inverse* kinematics. 4 | 5 | |Dimension![](media/useage-dimension.gif) |Forward Kinematics ![](media/useage-fk.gif) 6 | --- | --- 7 | 8 | |Inverse Kinematics![](media/useage-ik.gif) |Walking Gait ![](media/useage-gait.gif) 9 | --- | --- 10 | 11 | This project is implemented by Python with *numpy* for some matrix computations, *scipy.transforms* for 3D transformation, *dash* for user interface and interactive visualization, *matplotlib* for visualization/animation. 12 | 13 | ### Tutorial 14 | A [video tutorial](https://www.bilibili.com/video/BV1qF41167Sx) for building this project from scratch is available from [my channel](https://space.bilibili.com/13031745) on Bilibili (Chinese Language). 15 | 16 | ### How to use 17 | + just run the file `ui.py` and open on your browser, a web app will be hosted on http://127.0.0.1:8050, . 18 | + in `models.py`: you can run several visualizations/testings of the single models. 19 | + **Hexapod** 20 | ```python 21 | if __name__ == "__main__": 22 | hexapod = Hexapod() 23 | fig1, ax1 = hexapod.visualize3d() 24 | hexapod.solve_ik([0, 6, 2], [0, 0, 0.2]) 25 | print(hexapod.get_legs_pose()) 26 | fig2, ax2 = hexapod.visualize3d() 27 | plt.show() 28 | ``` 29 | This will print the leg positions (joint angles of each leg) of the hexapod: 30 | ```python 31 | {'MiddleRight': {'coxa': -92.09547174803812, 'femur': 6.36113571972891, 'tibia': -64.60434061668676}, 'FrontRight': {'coxa': -45.09639870158676, 'femur': 0.09531185287904773, 'tibia': -78.66206919332849}, 'FrontLeft': {'coxa': 45.10012726867826, 'femur': -12.077624233788663, 'tibia': -64.00686346144602}, 'MiddleLeft': {'coxa': 88.11133082594907, 'femur': -18.539795381247323, 'tibia': -49.1551576670496}, 'RearLeft': {'coxa': 130.89088550579493, 'femur': -11.540579859453953, 'tibia': -40.477079527074935}, 'RearRight': {'coxa': -139.3256544041924, 'femur': 0.7673781486253016, 'tibia': -44.45742377675202}} 32 | ``` 33 | 34 | Two figures of the hexapod's initial pose and the changed pose will also be displayed: 35 | |![](media/plt_ex1_initial.png) | ![](media/plt_ex1_ik.png) 36 | --- | --- 37 | 38 | + **Legs** 39 | ```python 40 | if __name__ == "__main__": 41 | leg = Leg(lengths=[2,2,3]) 42 | t = get_transformation_homo([0, 0, 45], [0,4,0]) 43 | leg.change_pose([-60, 45, 30]) 44 | leg.transform(t) 45 | fig, ax = leg.visualize3d() 46 | fig, ax = leg.visualizeAxis(fig=fig, ax=ax, scale=1) 47 | ``` 48 | this will display the leg 3D positions and the local axis: 49 | ![leg_pose](media/leg_pose1.png) | ![leg_pose](media/leg_pose2.png) 50 | ---|--- 51 | + `animation.py`: run this file to display animation of the hexapod, followings are examples of tripod gait and IK: 52 | ![tripod_gait](media/plt_ani_tripod_gait.gif) | ![body_ik](media/plt_body_ik.gif) 53 | ---|--- 54 | 55 | ### Issues 56 | + The angle range of every joint has not been limited, i.e., it can move from 0 to $2\pi$ 57 | + In the inverse kinematics page, the solution may be wrong when it exceeds the allowed tilt range like: 58 | 59 | 60 | 61 | 62 | Currently I have yet got an efficient way to address this issue, do you have any good idea? 63 | + Only Tripod gait has been implemented. Other gait will be realized in the [Webots project](https://github.com/XuelongSun/HexapodWebots): a simulated hexapod robot with physics engine. 64 | + Functions of some widgets (like the stance sliders in the IK tab) in the UI are not implemented. 65 | 66 | --- 67 | 68 | > This project is mainly inspired by [Mithi's Bare-Minimum Hexapod Robot Simulator](https://github.com/mithi/hexapod) 69 | 70 | > This project is a good example of `learn by doing`, have fun! 71 | 72 | 73 | -------------------------------------------------------------------------------- /animation.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from mpl_toolkits.mplot3d.art3d import Poly3DCollection 4 | from matplotlib.animation import FuncAnimation 5 | from models import Body, Leg, Hexapod 6 | 7 | fig = plt.figure(figsize=(10,10)) 8 | ax = fig.add_subplot(111, projection='3d') 9 | 10 | hexapod = Hexapod([3,2,3]) 11 | 12 | # draw body 13 | head = ax.scatter(hexapod.body.head.x, 14 | hexapod.body.head.y, 15 | hexapod.body.head.z, 16 | facecolors='red', edgecolors='tomato', s=hexapod.body.f*80, zorder=1) 17 | v = list([[v.x, v.y, v.z] for v in hexapod.body.vertices]) 18 | body = ax.add_collection3d(Poly3DCollection([v], facecolor='w', edgecolor='k', linewidth=8, zorder=0)) 19 | # draw legs 20 | legs = {} 21 | for k, leg in hexapod.legs.items(): 22 | px = [leg.points_global[i].x for i in range(4)] 23 | py = [leg.points_global[i].y for i in range(4)] 24 | pz = [leg.points_global[i].z for i in range(4)] 25 | l, = ax.plot(px, py, pz, lw=8, color='k', 26 | marker='o', markersize=10, mec='k', mfc='k') 27 | legs[k] = l 28 | 29 | tip_curve, = ax.plot([], [], lw=1, color='r') 30 | tip_curve_counter = 20 31 | tip_leg = 5 32 | tip_p = hexapod.legs[tip_leg].get_ground_contact_point() 33 | tip_points = [[tip_p.x], [tip_p.y], [tip_p.z]] 34 | 35 | t = np.linspace(0, np.pi*2, 100) 36 | rot_x = -10*np.sin(t) 37 | rot_y = -5*np.cos(t) 38 | rot_z = -5*np.sin(t) 39 | # ax.set_axis_off() 40 | hexapod.generate_walking_sequence(dict( 41 | Gait='Tripod', HipSwing=30, LiftSwing=60, StepNum=10, Direction=-1, Rotation=0 42 | )) 43 | 44 | def update(frame): 45 | global tip_points 46 | # IK 47 | hexapod.solve_ik([rot_x[frame%len(t)], rot_y[frame%len(t)], 0], [0, 0, 0]) 48 | # Gait 49 | # hexapod.set_pose_from_walking_sequence(frame%len(hexapod.walking_sequence[0]['coxia'])) 50 | # update body 51 | v = list([[v.x, v.y, v.z] for v in hexapod.body.vertices]) 52 | body.set_verts([v]) 53 | # update head 54 | head._offsets3d = ([hexapod.body.head.x], [hexapod.body.head.y], [hexapod.body.head.z]) 55 | # update legs 56 | for k, leg in hexapod.legs.items(): 57 | px = [leg.points_global[i].x for i in range(4)] 58 | py = [leg.points_global[i].y for i in range(4)] 59 | pz = [leg.points_global[i].z for i in range(4)] 60 | legs[k].set_data(px, py) 61 | legs[k].set_3d_properties(pz) 62 | # update tip curve 63 | tip_p = hexapod.legs[tip_leg].get_ground_contact_point() 64 | if frame == 0: 65 | tip_points = [[tip_p.x], [tip_p.y], [tip_p.z]] 66 | else: 67 | tip_points[0].append(tip_p.x) 68 | tip_points[1].append(tip_p.y) 69 | tip_points[2].append(tip_p.z) 70 | if len(tip_points[0]) < tip_curve_counter: 71 | tip_curve.set_data(tip_points[0], tip_points[1]) 72 | tip_curve.set_3d_properties(tip_points[2]) 73 | else: 74 | tip_curve.set_data(tip_points[0][-tip_curve_counter:-1], 75 | tip_points[1][-tip_curve_counter:-1]) 76 | tip_curve.set_3d_properties(tip_points[2][-tip_curve_counter:-1]) 77 | 78 | ax.set_aspect('equal') 79 | 80 | # ani = FuncAnimation(fig, update, frames=4*len(hexapod.walking_sequence[0]['coxia']), interval=20, blit=False) 81 | ani = FuncAnimation(fig, update, frames=4*len(t), interval=20, blit=False) 82 | plt.show() -------------------------------------------------------------------------------- /constant.py: -------------------------------------------------------------------------------- 1 | # useful 2 | AXIS_INDEX = {'X':0, 'Y':1, 'Z':2} 3 | 4 | # hexapod robot 5 | DEFAULT_DIMSIONS = (2, 4, 4) 6 | 7 | # legs 8 | DEFAULT_LEG_LENGTH = (2, 2, 2) 9 | DEFAULT_LEG_ALPHA_BIAS = (-90, -45, 45, 90, 135, -135) 10 | DEFAULT_LEG_GAMMA = -90 11 | LEG_ID_NAMES = {0: "MiddleRight", 1:"FrontRight", 2:"FrontLeft", 12 | 3: "MiddleLeft", 4:"RearLeft", 5:"RearRight"} 13 | LEG_NAMES_ID = {} 14 | for k,v in LEG_ID_NAMES.items(): 15 | LEG_NAMES_ID[v] = k 16 | LEG_SEG_ID_NAMES = {0:"coxa", 1:"femur", 2:"tibia"} 17 | LEG_SEG_NAMES_ID = {} 18 | for k, v in LEG_SEG_ID_NAMES.items(): 19 | LEG_SEG_NAMES_ID[v] = k 20 | 21 | GOOD_LEG_TRIOS = [ 22 | (0, 1, 3), 23 | (0, 1, 4), 24 | (0, 2, 3), 25 | (0, 2, 4), 26 | (0, 2, 5), 27 | (0, 3, 4), 28 | (0, 3, 5), 29 | (1, 2, 4), 30 | (1, 2, 5), 31 | (1, 3, 4), 32 | (1, 3, 5), 33 | (1, 4, 5), 34 | (2, 3, 5), 35 | (2, 4, 5), 36 | ] 37 | 38 | ADJACENT_LEG_TRIOS = [ 39 | (0, 1, 2), 40 | (1, 2, 3), 41 | (2, 3, 4), 42 | (3, 4, 5), 43 | (0, 4, 5), 44 | (0, 1, 5), 45 | ] 46 | 47 | LEG_TRIOS = GOOD_LEG_TRIOS + ADJACENT_LEG_TRIOS 48 | -------------------------------------------------------------------------------- /mathematics.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.spatial.transform import Rotation as R 3 | 4 | def combine_rot_trans_to_homo(rot, trans): 5 | M = np.identity(4) 6 | M[:3, :3] = rot 7 | M[:3, 3] = trans 8 | return M 9 | 10 | def get_transformation_homo(rot, trans, degrees=True): 11 | M = np.identity(4) 12 | M[:3, :3] = R.from_euler("XYZ", rot, degrees=degrees).as_matrix() 13 | # translation 14 | M[:3, 3] = trans 15 | return M 16 | 17 | def get_plane_norm(p0, p1, p2): 18 | '''get the unit vector of the plane's norm given three points 19 | ''' 20 | u = p1 - p0 21 | v = p2 - p0 22 | n = np.cross(u, v) 23 | n = n / np.linalg.norm(n) 24 | return n 25 | 26 | def is_point_within_triangle_same_plane(point, triangle): 27 | '''determine if the point is within the triangle formed by three points 28 | point is already in the plane formed defined by the triangle 29 | ''' 30 | AB = triangle[1]-triangle[0] 31 | AC = triangle[2]-triangle[0] 32 | PA = point - triangle[0] 33 | PB = point - triangle[1] 34 | PC = point - triangle[2] 35 | area_triangle = np.linalg.norm(np.cross(AB, AC)) 36 | alpha = np.linalg.norm(np.cross(PB, PC))/area_triangle 37 | beta = np.linalg.norm(np.cross(PC, PA))/area_triangle 38 | gamma = 1 - alpha - beta 39 | return (0 < alpha < 1) and (0 < beta < 1) and (0 < gamma < 1) 40 | 41 | # https://math.stackexchange.com/questions/544946/determine-if-projection-of-3d-point-onto-plane-is-within-a-triangle 42 | # https://gamedev.stackexchange.com/questions/23743/whats-the-most-efficient-way-to-find-barycentric-coordinates 43 | # https://en.wikipedia.org/wiki/Barycentric_coordinate_system 44 | def is_projected_point_within_triangle(point, triangle): 45 | '''determine if the point is within the triangle formed by three points 46 | by applying the Barycentric coordinates 47 | ''' 48 | u = triangle[1]-triangle[0] 49 | v = triangle[2]-triangle[0] 50 | w = point - triangle[0] 51 | n = np.cross(u, v) 52 | alpha = np.linalg.norm(np.cross(u, w).dot(n))/n.dot(n) 53 | beta = np.linalg.norm(np.cross(w, v).dot(n))/n.dot(n) 54 | gamma = 1 - alpha - beta 55 | return (0 < alpha < 1) and (0 < beta < 1) and (0 < gamma < 1) 56 | 57 | # https://stackoverflow.com/questions/45142959/calculate-rotation-matrix-to-align-two-vectors-in-3d-space 58 | # https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d 59 | def get_rotation_matrix_align_vectors(vec1, vec2): 60 | """ Find the rotation matrix that aligns vec1 to vec2 61 | :param vec1: A 3d "source" vector 62 | :param vec2: A 3d "destination" vector 63 | :return mat: A transform matrix (3x3) which when applied to vec1, aligns it with vec2. 64 | """ 65 | a, b = (vec1 / np.linalg.norm(vec1)).reshape(3), (vec2 / np.linalg.norm(vec2)).reshape(3) 66 | v = np.cross(a, b) 67 | c = np.dot(a, b) 68 | s = np.linalg.norm(v) 69 | if s == 0.0: 70 | return np.eye(3) 71 | kmat = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]]) 72 | rotation_matrix = np.eye(3) + kmat + kmat.dot(kmat) * ((1 - c) / (s ** 2)) 73 | return rotation_matrix 74 | 75 | # https://www.maplesoft.com/support/help/Maple/view.aspx?path=MathApps%2FProjectionOfVectorOntoPlane 76 | def project_vector_onto_plane(vec, plane_norm): 77 | s = vec.dot(plane_norm) / plane_norm.dot(plane_norm) 78 | return vec - s*plane_norm 79 | 80 | def vector_angle(v1, v2, degree=True): 81 | v = v1.dot(v2)/np.sqrt(v1.dot(v1)*v2.dot(v2)) 82 | if abs(v) <= 1: 83 | a = np.arccos(v) 84 | else: 85 | a = 0.0 86 | 87 | return a if not degree else np.rad2deg(a) 88 | 89 | def can_form_triangle(a, b, c): 90 | return (a + b > c) and (a + c > b) and (b + c > a) 91 | 92 | class Point3D: 93 | def __init__(self, x=0, y=0, z=0, name='None') -> None: 94 | self.x = x 95 | self.y = y 96 | self.z = z 97 | self.name = name 98 | 99 | def get_coordinates(self): 100 | return self.x, self.y, self.z 101 | 102 | def set_coordinates(self, coordinates): 103 | if hasattr(coordinates, '__len__'): 104 | if len(coordinates) >= 3: 105 | self.x = coordinates[0] 106 | self.y = coordinates[1] 107 | self.z = coordinates[2] 108 | else: 109 | raise ValueError 110 | else: 111 | raise ValueError 112 | 113 | def get_coordinates_homo(self): 114 | return self.x, self.y, self.z, 1 115 | 116 | def __repr__(self): 117 | s = f"{self.name}:({self.x:>4.2f}, {self.y:>4.2f}, {self.z:>4.2f})" 118 | return s 119 | 120 | def __add__(self, b): 121 | if isinstance(b, Point3D): 122 | return Point3D(self.x + b.x, self.y + b.y, self.z + b.z, name=self.name) 123 | elif isinstance(b, float) or isinstance(b, int): 124 | return Point3D(self.x + b, self.y + b, self.z + b, name=self.name) 125 | elif hasattr(b, "__len__"): 126 | if len(b) == 3: 127 | return Point3D(self.x + b[0], self.y + b[1], self.z + b[2], name=self.name) 128 | else: 129 | raise ValueError 130 | 131 | def __sub__ (self, b): 132 | if isinstance(b, Point3D): 133 | return Point3D(self.x - b.x, self.y - b.y, self.z - b.z, name=self.name + '-' + b.name) 134 | elif isinstance(b, float) or isinstance(b, int): 135 | return Point3D(self.x - b, self.y - b, self.z - b, name=self.name + '-' + str(b)) 136 | else: 137 | raise ValueError 138 | 139 | def dot(self, b): 140 | if isinstance(b, Point3D): 141 | return self.x*b.x + self.y*b.y + self.z*b.z 142 | else: 143 | raise ValueError -------------------------------------------------------------------------------- /media/ik-issue.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XuelongSun/HexapodRobotSimulation/355c09ef1f753a7c4c9796f9d3f1f87a706ddcc2/media/ik-issue.gif -------------------------------------------------------------------------------- /media/leg_pose1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XuelongSun/HexapodRobotSimulation/355c09ef1f753a7c4c9796f9d3f1f87a706ddcc2/media/leg_pose1.png -------------------------------------------------------------------------------- /media/leg_pose2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XuelongSun/HexapodRobotSimulation/355c09ef1f753a7c4c9796f9d3f1f87a706ddcc2/media/leg_pose2.png -------------------------------------------------------------------------------- /media/plt_ani_tripod_gait.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XuelongSun/HexapodRobotSimulation/355c09ef1f753a7c4c9796f9d3f1f87a706ddcc2/media/plt_ani_tripod_gait.gif -------------------------------------------------------------------------------- /media/plt_body_ik.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XuelongSun/HexapodRobotSimulation/355c09ef1f753a7c4c9796f9d3f1f87a706ddcc2/media/plt_body_ik.gif -------------------------------------------------------------------------------- /media/plt_ex1_ik.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XuelongSun/HexapodRobotSimulation/355c09ef1f753a7c4c9796f9d3f1f87a706ddcc2/media/plt_ex1_ik.png -------------------------------------------------------------------------------- /media/plt_ex1_initial.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XuelongSun/HexapodRobotSimulation/355c09ef1f753a7c4c9796f9d3f1f87a706ddcc2/media/plt_ex1_initial.png -------------------------------------------------------------------------------- /media/useage-dimension.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XuelongSun/HexapodRobotSimulation/355c09ef1f753a7c4c9796f9d3f1f87a706ddcc2/media/useage-dimension.gif -------------------------------------------------------------------------------- /media/useage-fk.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XuelongSun/HexapodRobotSimulation/355c09ef1f753a7c4c9796f9d3f1f87a706ddcc2/media/useage-fk.gif -------------------------------------------------------------------------------- /media/useage-gait.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XuelongSun/HexapodRobotSimulation/355c09ef1f753a7c4c9796f9d3f1f87a706ddcc2/media/useage-gait.gif -------------------------------------------------------------------------------- /media/useage-ik.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XuelongSun/HexapodRobotSimulation/355c09ef1f753a7c4c9796f9d3f1f87a706ddcc2/media/useage-ik.gif -------------------------------------------------------------------------------- /models.py: -------------------------------------------------------------------------------- 1 | from copy import deepcopy 2 | 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | from matplotlib.patches import Polygon 6 | from mpl_toolkits.mplot3d.art3d import Poly3DCollection 7 | from scipy.spatial.transform import Rotation as R 8 | 9 | from constant import * 10 | from mathematics import * 11 | 12 | class Body: 13 | # |-f-| 14 | # P2---*---P1-------- 15 | # / | \ | 16 | # / | \ s 17 | # / | \ | 18 | # P3-------------P0 --- 19 | # \ | /| 20 | # \ | / | 21 | # \ | / | 22 | # P4--*--P5 | 23 | # | | 24 | # |---m---| 25 | # y axis 26 | # ^ 27 | # | 28 | # | 29 | # ----> x axis 30 | def __init__(self, f=5, s=10, m=10) -> None: 31 | self.init_vertices((f,m,s)) 32 | 33 | def init_vertices(self, dim): 34 | self.f, self.s, self.m = dim 35 | self.vertices = [ 36 | Point3D(self.m, 0, 0, 'P0'), 37 | Point3D(self.f, self.s, 0, 'P1'), 38 | Point3D(-self.f, self.s, 0, 'P2'), 39 | Point3D(-self.m, 0, 0, 'P3'), 40 | Point3D(-self.f, -self.s, 0, 'P4'), 41 | Point3D(self.f, -self.s, 0, 'P5') 42 | ] 43 | self.cog = Point3D(0, 0, 0, 'COG') 44 | self.head = Point3D(0, self.s, 0, 'Head') 45 | 46 | def translate(self, offset): 47 | if hasattr(offset, "__len__"): 48 | if len(offset) == 3: 49 | for p in self.vertices + [self.cog, self.head]: 50 | p.set_coordinates([p.x + offset[0], p.y + offset[1], p.z + offset[2]]) 51 | else: 52 | raise ValueError 53 | else: 54 | raise ValueError 55 | 56 | def rotate(self, rot): 57 | if hasattr(rot, "__len__"): 58 | if len(rot) == 3: 59 | r = R.from_euler('XYZ', rot, degrees=True).as_matrix() 60 | for p in self.vertices + [self.cog, self.head]: 61 | p.set_coordinates(np.matmul(r, p.get_coordinates())) 62 | else: 63 | raise ValueError 64 | else: 65 | raise ValueError 66 | 67 | def transform(self, transform): 68 | for v in self.vertices: 69 | v.set_coordinates(transform.dot(v.get_coordinates_homo())) 70 | self.head.set_coordinates(transform.dot(self.head.get_coordinates_homo())) 71 | self.cog.set_coordinates(transform.dot(self.cog.get_coordinates_homo())) 72 | 73 | def change_dimensions(self, dimension): 74 | self.init_vertices(dimension) 75 | 76 | def visualize2d(self, fig=None, ax=None): 77 | if fig is None: 78 | fig, ax = plt.subplots() 79 | # add head 80 | ax.scatter(self.head.x, self.head.y, facecolors='red', edgecolors='tomato', alpha=0.7, s=self.f*20) 81 | ax.text(self.head.x, self.head.y, 'Head') 82 | # add center of gravity 83 | ax.scatter(self.cog.x, self.cog.y, facecolors='k', edgecolors='gray', alpha=0.7, s=self.f*10) 84 | ax.text(self.cog.x, self.cog.y, 'COG') 85 | # add body hexagon 86 | v = [(v.x, v.y) for v in self.vertices] 87 | body = Polygon(v, facecolor='skyblue', alpha=0.6, fill=True, edgecolor='darkblue') 88 | ax.add_patch(body) 89 | # add point label 90 | for v in self.vertices: 91 | ax.text(v.x, v.y, v.name) 92 | 93 | # adjuestment 94 | ax.set_xlim([self.cog.x-1.5*self.m, self.cog.x + 1.5*self.m]) 95 | ax.set_ylim([self.cog.y-1.5*self.s, self.cog.y + 1.5*self.s]) 96 | ax.grid() 97 | ax.set_aspect('equalxy') 98 | return fig, ax 99 | 100 | def visualize3d(self, fig=None, ax=None): 101 | if fig is None: 102 | fig = plt.figure() 103 | ax = fig.add_subplot(projection="3d", proj_type="ortho") 104 | # add head 105 | ax.scatter(self.head.x, self.head.y, self.head.z, facecolors='red', edgecolors='tomato', alpha=0.7, s=self.f*20) 106 | ax.text(self.head.x, self.head.y, self.head.z, 'Head') 107 | # add center of gravity 108 | ax.scatter(self.cog.x, self.cog.y, self.cog.z, facecolors='k', edgecolors='gray', alpha=0.7, s=self.f*10) 109 | ax.text(self.cog.x, self.cog.y, self.cog.z, 'COG') 110 | v = list([[v.x, v.y, v.z] for v in self.vertices]) 111 | ax.add_collection3d(Poly3DCollection([v], facecolor='skyblue', alpha=0.6, edgecolor='darkblue', linewidth=5)) 112 | ax.set_xlim([self.cog.x-1.5*self.m, self.cog.x + 1.5*self.m]) 113 | ax.set_ylim([self.cog.y-1.5*self.s, self.cog.y + 1.5*self.s]) 114 | ax.set_zlim([-5, 5]) 115 | ax.set_aspect('equal') 116 | return fig, ax 117 | 118 | 119 | class Leg: 120 | # |--lengths[0]-|--lengths[1]--| 121 | # |=============|==============| p2 ------- 122 | # p0 p1 | | 123 | # (origin) | | 124 | # | lengths[2] 125 | # | | 126 | # | | 127 | # | p3 ------- 128 | # z axis 129 | # | 130 | # | 131 | # |------- y axis 132 | # origin 133 | def __init__(self, lengths=[10, 10, 10], 134 | name='none', joint_num=3) -> None: 135 | self.joint_num = joint_num 136 | self.name=name 137 | 138 | self.lengths = lengths 139 | self.angles = [0, 0, 0] 140 | self.reset_pose() 141 | self.reset_transforms() 142 | self.global_transform = np.identity(4) 143 | # limitations of the angels / degrees 144 | self.angle_limits = [(-180, 180), (-180, 180), (-180, 180)] 145 | 146 | def __repr__(self): 147 | s = f"Leg:{self.name}({self.origin})" 148 | return s 149 | 150 | def reset_pose(self): 151 | self.angles = [0, 0, 0] 152 | p0 = Point3D(0, 0, 0, name='P0-BodyContact') 153 | p1 = Point3D(0, self.lengths[0], 0, name='P1-coxa') 154 | p2 = Point3D(0, self.lengths[0] + self.lengths[1], 0, name='P2-femur') 155 | p3 = Point3D(0, self.lengths[0] + self.lengths[1] + self.lengths[2], 0, name='P3-tibia') 156 | self.points_global = [p0, p1, p2, p3] 157 | 158 | def reset_transforms(self): 159 | # axes 160 | t_p0 = get_transformation_homo([0, 0, 0], [0, 0, 0]) 161 | t_p1_p0 = get_transformation_homo([0, 0, 0], [0, self.lengths[0], 0]) 162 | # t_p1 = np.matmul(t_p0, t_p1_p0) 163 | t_p1 = t_p1_p0.dot(t_p0) 164 | t_p2_p1 = get_transformation_homo([0, 0, 0], [0, self.lengths[1], 0]) 165 | # t_p2 = np.matmul(t_p1, t_p2_p1) 166 | t_p2 = t_p2_p1.dot(t_p1) 167 | t_p3_p2 = get_transformation_homo([0, 0, 0], [0, self.lengths[2], 0]) 168 | # t_p3 = np.matmul(t_p2, t_p3_p2) 169 | t_p3 = t_p3_p2.dot(t_p2) 170 | self.transforms = [t_p0, t_p1, t_p2, t_p3] 171 | return self.transforms 172 | 173 | def _update_transforms(self): 174 | t_p0 = get_transformation_homo([0, 0, self.angles[0]], [0, 0, 0]) 175 | t_p1_p0 = get_transformation_homo([self.angles[1], 0, 0], [0, self.lengths[0], 0]) 176 | t_p1 = t_p0.dot(t_p1_p0) 177 | t_p2_p1 = get_transformation_homo([self.angles[2], 0, 0], [0, self.lengths[1], 0]) 178 | t_p2 = t_p1.dot(t_p2_p1) 179 | t_p3_p2 = get_transformation_homo([0, 0, 0], [0, self.lengths[2], 0]) 180 | t_p3 = t_p2.dot(t_p3_p2) 181 | self.transforms = [t_p0, t_p1, t_p2, t_p3] 182 | 183 | def _update_pose(self): 184 | for p, t in zip(self.points_global, self.transforms): 185 | p.set_coordinates(self.global_transform.dot(t.dot([0, 0, 0, 1]))) 186 | p.name = 'BodyCOG-' + p.name 187 | 188 | def change_pose(self, angles): 189 | self.angles = angles 190 | self._update_transforms() 191 | self._update_pose() 192 | return self.points_global 193 | 194 | def get_ground_contact_point(self): 195 | # the lowest point as the ground contact 196 | # usual case is the end point of the leg, i.e., tibia 197 | self.ground_contact_point = self.points_global[-1] 198 | for p in self.points_global[::-1]: 199 | if p.z < self.ground_contact_point.z: 200 | self.ground_contact_point = p 201 | return self.ground_contact_point 202 | 203 | def transform(self, transform): 204 | self.global_transform = np.matmul(transform, self.global_transform) 205 | # self.global_transform = transform 206 | for p in self.points_global: 207 | p.set_coordinates(transform.dot(p.get_coordinates_homo())) 208 | 209 | def solve_ik(self, start_p, end_p): 210 | vec_p0_p3 = end_p - start_p 211 | vec_p0_p3_len = np.linalg.norm(vec_p0_p3) 212 | # coxa vector is the projection of the P0->P3 onto the xy-plane 213 | coxa_vec = project_vector_onto_plane(vec_p0_p3, np.array((0, 0, 1))) 214 | coxa_vec_unit = coxa_vec/np.linalg.norm(coxa_vec) 215 | coxa_vec = coxa_vec_unit*self.lengths[0] 216 | p1 = coxa_vec + start_p 217 | vec_p0_p1 = p1 - start_p 218 | alpha = vector_angle(coxa_vec_unit, np.array([0, 1, 0])) 219 | if coxa_vec[0] > 0: 220 | alpha *= -1 221 | rho = vector_angle(coxa_vec, vec_p0_p3, degree=False) 222 | if vec_p0_p3[-1] < 0: 223 | rho*=-1 224 | loc_p3y = vec_p0_p3_len * np.cos(rho) 225 | loc_p3z = vec_p0_p3_len * np.sin(rho) 226 | 227 | vec_p1_p3 = end_p - p1 228 | vec_p1_p3_len = np.linalg.norm(vec_p1_p3) 229 | 230 | if not can_form_triangle(vec_p1_p3_len, self.lengths[1], self.lengths[2]): 231 | # cannot reach the goal, so stretch the segments on the same line 232 | vec_p1_p2 = vec_p1_p3/vec_p1_p3_len*self.lengths[1] 233 | vec_p2_p3 = vec_p1_p3/vec_p1_p3_len*self.lengths[2] 234 | p2 = p1 + vec_p1_p2 235 | p3 = p2 + vec_p2_p3 236 | gamma = 0 237 | beta = vector_angle(vec_p0_p1, vec_p1_p2) 238 | else: 239 | # could form the triangle, use cosine theorem to get the angle between 240 | theta = np.arccos((vec_p1_p3_len**2 + self.lengths[1]**2 - self.lengths[2]**2)/(2*vec_p1_p3_len*self.lengths[1])) 241 | phi = vector_angle(vec_p1_p3, vec_p0_p1, degree=False) 242 | # different cases for the relationship of beta, phi and theta 243 | beta = theta - phi if loc_p3z < 0 else theta + phi 244 | loc_p2z = self.lengths[1]*np.sin(beta) 245 | loc_p2y = vec_p0_p1[1] + self.lengths[1]*np.cos(beta) 246 | vec_p1_p2 = np.array([0, loc_p2y, loc_p2z]) - vec_p0_p1 247 | p2 = p1 + vec_p1_p2 248 | vec_p2_p3 = np.array([0, loc_p3y, loc_p3z]) - np.array([0, loc_p2y, loc_p2z]) 249 | p3 = p2 + vec_p2_p3 250 | gamma = vector_angle(vec_p2_p3, vec_p1_p2) 251 | if loc_p2z > loc_p3z: 252 | gamma *= -1 253 | beta = np.rad2deg(beta) 254 | 255 | diff = start_p - np.array(self.points_global[0].get_coordinates()) 256 | self.global_transform = np.matmul(get_transformation_homo([0,0,0], diff), self.global_transform) 257 | 258 | # assign to the leg's attributes 259 | self.change_pose([alpha, beta, gamma]) 260 | 261 | self.points_global[-1].set_coordinates(end_p) 262 | return alpha, beta, gamma 263 | 264 | def visualize3d(self, fig=None, ax=None): 265 | if fig is None: 266 | fig = plt.figure() 267 | ax = fig.add_subplot(projection="3d", proj_type="persp") 268 | # Points P0-04 269 | for i, p in enumerate(self.points_global): 270 | if i == 0: 271 | color='r' 272 | else: 273 | color='k' 274 | ax.scatter(p.x, p.y, p.z, s=100, color=color) 275 | for l in [(0,1),(1,2),(2,3)]: 276 | px = [self.points_global[l[0]].x, self.points_global[l[1]].x] 277 | py = [self.points_global[l[0]].y, self.points_global[l[1]].y] 278 | pz = [self.points_global[l[0]].z, self.points_global[l[1]].z] 279 | ax.plot(px, py, pz, lw=10, color='royalblue', alpha=0.6) 280 | ax.set_xlabel('x') 281 | ax.set_ylabel('y') 282 | ax.set_zlabel('z') 283 | ax.set_aspect('equal') 284 | return fig, ax 285 | 286 | def visualizeAxis(self, axis='0123', scale=1, fig=None, ax=None): 287 | def plot_axis(ax, global_transform, transform, scale, text='axis'): 288 | px = [scale,0,0,1] 289 | py = [0,scale,0,1] 290 | pz = [0,0,scale,1] 291 | po = [0,0,0,1] 292 | px_t = global_transform.dot(transform.dot(px)) 293 | py_t = global_transform.dot(transform.dot(py)) 294 | pz_t = global_transform.dot(transform.dot(pz)) 295 | po_t = global_transform.dot(transform.dot(po)) 296 | 297 | xline, = ax.plot([po_t[0], px_t[0]], [po_t[1], px_t[1]], [po_t[2], px_t[2]], color='red', lw=2) 298 | yline, = ax.plot([po_t[0], py_t[0]], [po_t[1], py_t[1]], [po_t[2], py_t[2]], color='green', lw=2) 299 | zline, = ax.plot([po_t[0], pz_t[0]], [po_t[1], pz_t[1]], [po_t[2], pz_t[2]], color='blue', lw=2) 300 | ax.text(po_t[0], po_t[1], po_t[2], text) 301 | return ax 302 | 303 | if fig is None: 304 | fig = plt.figure() 305 | ax = fig.add_subplot(projection="3d") 306 | for plot_s in axis: 307 | ax = plot_axis(ax, self.global_transform, self.transforms[int(plot_s)], scale, text='P' + plot_s) 308 | ax = plot_axis(ax, np.identity(4), np.identity(4), scale, text='world') 309 | return fig, ax 310 | 311 | 312 | class Hexapod: 313 | def __init__(self, leg_length=DEFAULT_LEG_LENGTH): 314 | self.body = Body(*DEFAULT_DIMSIONS) 315 | self.legs = {} 316 | self.leg_alpha_bias = DEFAULT_LEG_ALPHA_BIAS 317 | self.init_state(DEFAULT_DIMSIONS, leg_length) 318 | 319 | self.walking_sequence = {} 320 | self.generate_walking_sequence(dict(Gait='Tripod', HipSwing=30, LiftSwing=20, StepNum=6, 321 | Direction=1, Rotation=0)) 322 | 323 | def init_state(self, body_dim, leg_length): 324 | self.initial_pose = {} 325 | self.body.init_vertices(body_dim) 326 | self.init_legs(leg_length) 327 | self.x_axis = Point3D(1, 0, 0, name='hexapod_x_axis') 328 | self.y_axis = Point3D(0, 1, 0, name='hexapod_y_axis') 329 | self.z_axis = Point3D(0, 0, 1, name='hexapod_z_axis') 330 | self.transform_m = np.identity(4) 331 | self.ground_contact_points = {} 332 | for k, v in self.legs.items(): 333 | self.ground_contact_points[k] = v.get_ground_contact_point() 334 | self.ground_contact_points_old = deepcopy(self.ground_contact_points) 335 | self.body_plane_height = 0 336 | return self.update_state() 337 | 338 | def init_legs(self, lengths, init_gamma=DEFAULT_LEG_GAMMA): 339 | for k, v in LEG_ID_NAMES.items(): 340 | self.legs[k] = Leg(lengths=lengths, name=v) 341 | self.legs[k].global_transform = get_transformation_homo([0, 0, 0], self.body.vertices[k].get_coordinates()) 342 | self.legs[k].angles = [self.leg_alpha_bias[k], 0, init_gamma] 343 | self.initial_pose[v] = {'coxa':self.leg_alpha_bias[k], 344 | 'femur':0, 345 | 'tibia':init_gamma} 346 | self.legs[k]._update_transforms() 347 | self.legs[k]._update_pose() 348 | 349 | def get_legs_pose(self): 350 | pose = {} 351 | for leg_id, leg in self.legs.items(): 352 | leg_dict = {} 353 | for k, v in LEG_SEG_NAMES_ID.items(): 354 | leg_dict[k] = leg.angles[v] 355 | pose[LEG_ID_NAMES[leg_id]] = leg_dict 356 | return pose 357 | 358 | def update_axis(self, transform=np.identity(4)): 359 | self.x_axis.set_coordinates(transform.dot(self.x_axis.get_coordinates_homo())) 360 | self.y_axis.set_coordinates(transform.dot(self.y_axis.get_coordinates_homo())) 361 | self.z_axis.set_coordinates(transform.dot(self.z_axis.get_coordinates_homo())) 362 | 363 | def transform(self, transform): 364 | self.transform_m = transform.dot(self.transform_m) 365 | self.body.transform(transform) 366 | for l in self.legs.values(): 367 | l.transform(transform) 368 | self.update_axis(transform) 369 | 370 | def body_transform(self, transform): 371 | self.transform_m = transform.dot(self.transform_m) 372 | self.body.transform(transform) 373 | self.update_axis(transform) 374 | 375 | def update_leg_pose(self, poses:dict): 376 | '''poses is a dict of dicts in format: {"leg_name":{"leg_seg_name":x degree}} 377 | ''' 378 | self.ground_contact_points_old = deepcopy(self.ground_contact_points) 379 | # determine if the hexapod should twist along z-axis due to 380 | # the change of alpha angle of the legs on the ground 381 | body_might_twist = False 382 | cnt = 0 383 | for n in self.ground_contact_points: 384 | if n in poses.keys(): 385 | if 'coxa' in poses[n]: 386 | if abs(poses[n]['coxa'] - self.legs[n].angles[0]-self.initial_pose[n][0]) > 0: 387 | cnt += 1 388 | if cnt >= 3: 389 | body_might_twist = True 390 | break 391 | 392 | for leg, v in poses.items(): 393 | for leg_seg, angle in v.items(): 394 | self.legs[LEG_NAMES_ID[leg]].angles[LEG_SEG_NAMES_ID[leg_seg]] = self.initial_pose[leg][leg_seg] + angle 395 | self.legs[LEG_NAMES_ID[leg]]._update_transforms() 396 | self.legs[LEG_NAMES_ID[leg]]._update_pose() 397 | 398 | return self.update_state(body_might_twist) 399 | 400 | def update_leg_pattern(self, angles): 401 | self.ground_contact_points_old = deepcopy(self.ground_contact_points) 402 | cnt = 0 403 | # all the legs share the same pose 404 | for i, l in enumerate(self.legs.values()): 405 | if abs(angles[0] - DEFAULT_LEG_ALPHA_BIAS[i] - l.angles[0]) > 0: 406 | cnt += 1 407 | l.angles = [angles[0] + DEFAULT_LEG_ALPHA_BIAS[i], angles[1], angles[2] + DEFAULT_LEG_GAMMA] 408 | l._update_transforms() 409 | l._update_pose() 410 | 411 | return self.update_state(body_might_twist=(cnt>=3)) 412 | 413 | def update_dimensions(self, dimension): 414 | '''dimension is a list: [f, m, s, coxa, femur, tibia] 415 | ''' 416 | return self.init_state(dimension[:3], dimension[3:]) 417 | 418 | def update_state(self, body_might_twist=False): 419 | '''update the pose of the robot 420 | ''' 421 | is_stable = False 422 | # find the ground contact point constructing the support polygon 423 | for leg_inds in LEG_TRIOS: 424 | p0, p1, p2 = [self.legs[i].get_ground_contact_point() for i in leg_inds] 425 | 426 | # justify if the cog is in the triangle formed by these legs 427 | if not is_projected_point_within_triangle(np.array(self.body.cog.get_coordinates()), 428 | [np.array(p0.get_coordinates()), 429 | np.array(p1.get_coordinates()), 430 | np.array(p2.get_coordinates())]): 431 | continue 432 | n = get_plane_norm(np.array(p0.get_coordinates()), 433 | np.array(p1.get_coordinates()), 434 | np.array(p2.get_coordinates())) 435 | # get distance from the cog to the support polygon plane 436 | d = n.dot((self.body.cog - p0).get_coordinates()) 437 | 438 | # check if this trio constructs the lowest plane (i.e., the biggest d) 439 | others = [self.legs[i].get_ground_contact_point() for i in set(LEG_ID_NAMES.keys()) - set(leg_inds)] 440 | r = True 441 | for p in others: 442 | d_ = n.dot((self.body.cog - p).get_coordinates()) 443 | if d_ > d: 444 | r = False 445 | if r: 446 | is_stable = True 447 | self.body_plane_norm = n 448 | break 449 | 450 | if is_stable: 451 | self.ground_contact_points = {} 452 | for leg_id in leg_inds: 453 | self.ground_contact_points[leg_id] = self.legs[leg_id].get_ground_contact_point() 454 | # get all the legs' end tip that contacts the ground 455 | for leg_id in set(LEG_ID_NAMES.keys()) - set(leg_inds): 456 | for p in reversed(self.legs[leg_id].points_global[1:]): 457 | d_ = n.dot((self.body.cog - p).get_coordinates()) 458 | if np.isclose(d, d_): 459 | self.ground_contact_points[leg_id] = p 460 | break 461 | # tilt the hexapod according to the new plane norm 462 | rot_m = get_rotation_matrix_align_vectors(self.body_plane_norm, np.array([0, 0, 1])) 463 | t = combine_rot_trans_to_homo(rot_m, [0, 0, d - self.body_plane_height]) 464 | self.transform(t) 465 | self.body_plane_height = d 466 | # twist body if needed 467 | if body_might_twist: 468 | # find one pair of point to get the twist angle 469 | for k, v in self.ground_contact_points_old.items(): 470 | if k in self.ground_contact_points: 471 | a = np.arctan2(v.y, v.x)-np.arctan2(self.ground_contact_points[k].y, self.ground_contact_points[k].x) 472 | t = get_transformation_homo([0,0,np.rad2deg(a)],[0,0,0]) 473 | self.transform(t) 474 | break 475 | return True 476 | else: 477 | print('The pose is not stable, keep previous pose') 478 | return False 479 | 480 | def solve_ik(self, rot, trans): 481 | # reset hexapod 482 | self.init_state((self.body.f, self.body.m, self.body.s), self.legs[0].lengths) 483 | # restore old body contacts 484 | old_body_contacts = deepcopy(self.body.vertices) 485 | # transform body to get new body contacts 486 | self.body_transform(get_transformation_homo(rot, trans)) 487 | # solve IK for each leg 488 | for k, leg in self.legs.items(): 489 | leg.solve_ik(np.array(self.body.vertices[k].get_coordinates()), 490 | np.array(self.ground_contact_points[k].get_coordinates())) 491 | 492 | def generate_walking_sequence(self, parameters:dict): 493 | gait = parameters['Gait'] 494 | d_alpha = parameters['HipSwing'] 495 | d_beta = parameters['LiftSwing'] 496 | d_gamma = -parameters['LiftSwing']/2 497 | step_num = parameters['StepNum'] 498 | move_dir = parameters['Direction'] 499 | rotation = parameters['Rotation'] 500 | self.init_state((self.body.f, self.body.m, self.body.s), self.legs[0].lengths) 501 | self.walking_sequence = {} 502 | if gait == 'Tripod': 503 | for k, leg in self.legs.items(): 504 | beta_s = np.linspace(0, d_beta, int(step_num)) 505 | beta_s_r = beta_s[::-1] 506 | beta_s_0 = np.ones(int(len(beta_s)*2))*beta_s[0] 507 | gamma_s = np.linspace(0, d_gamma, int(step_num)) 508 | gamma_s_r = gamma_s[::-1] 509 | gamma_s_0 = np.ones(int(len(gamma_s)*2))*gamma_s[0] 510 | if rotation != 0: 511 | alpha_s = np.linspace(-d_alpha, d_alpha, int(step_num*2)) 512 | alpha_s_r = alpha_s[::-1] 513 | else: 514 | if k in (2, 3, 4): 515 | alpha_s = np.linspace(d_alpha, -d_alpha, int(step_num*2)) 516 | alpha_s_r = alpha_s[::-1] 517 | else: 518 | alpha_s = np.linspace(-d_alpha, d_alpha, int(step_num*2)) 519 | alpha_s_r = alpha_s[::-1] 520 | alpha_seq_a = np.hstack([alpha_s, alpha_s_r]) 521 | alpha_seq_b = np.hstack([alpha_s_r, alpha_s]) 522 | if k in (0, 2, 4): 523 | self.walking_sequence[k] = {'coxa':alpha_seq_a, 524 | 'femur':np.hstack([beta_s, beta_s_r, beta_s_0]), 525 | 'tibia':np.hstack([gamma_s, gamma_s_r, gamma_s_0])} 526 | else: 527 | self.walking_sequence[k] = {'coxa':alpha_seq_b, 528 | 'femur':np.hstack([beta_s_0, beta_s, beta_s_r]), 529 | 'tibia':np.hstack([gamma_s_0, gamma_s, gamma_s_r])} 530 | elif gait == 'Ripple': 531 | pass 532 | 533 | def set_pose_from_walking_sequence(self, step): 534 | poses = {} 535 | for k, v in self.walking_sequence.items(): 536 | P = {} 537 | for seg in LEG_SEG_NAMES_ID.keys(): 538 | P[seg] = v[seg][step] 539 | poses[LEG_ID_NAMES[k]] = P 540 | return self.update_leg_pose(poses) 541 | 542 | 543 | def visualize3d(self, fig=None, ax=None): 544 | if fig is None: 545 | fig = plt.figure() 546 | ax = fig.add_subplot(projection="3d") 547 | fig, ax = self.body.visualize3d(fig, ax) 548 | for k, leg in self.legs.items(): 549 | fig, ax = leg.visualize3d(fig, ax) 550 | # hexapod axis 551 | ax.plot([self.body.cog.x, self.x_axis.x], 552 | [self.body.cog.y, self.x_axis.y], 553 | [self.body.cog.z, self.x_axis.z], 554 | color='red') 555 | ax.plot([self.body.cog.x, self.y_axis.x], 556 | [self.body.cog.y, self.y_axis.y], 557 | [self.body.cog.z, self.y_axis.z], 558 | color='green') 559 | ax.plot([self.body.cog.x, self.z_axis.x], 560 | [self.body.cog.y, self.z_axis.y], 561 | [self.body.cog.z, self.z_axis.z], 562 | color='blue') 563 | ax.set_aspect('equal') 564 | ax.set_zlim([0,6]) 565 | return fig, ax 566 | 567 | 568 | if __name__ == "__main__": 569 | # hexapod = Hexapod() 570 | # fig1, ax1 = hexapod.visualize3d() 571 | # hexapod.solve_ik([0, 6, 2], [0, 0, 0.2]) 572 | # print(hexapod.get_legs_pose()) 573 | # fig2, ax2 = hexapod.visualize3d() 574 | # plt.show() 575 | 576 | # leg = Leg(lengths=[2,2,3]) 577 | # t = get_transformation_homo([0, 0, 45], [0,4,0]) 578 | # leg.change_pose([-60, 45, 30]) 579 | # leg.transform(t) 580 | # fig, ax = leg.visualize3d() 581 | # fig, ax = leg.visualizeAxis(fig=fig, ax=ax, scale=1) 582 | # plt.show() 583 | pass -------------------------------------------------------------------------------- /style.ini: -------------------------------------------------------------------------------- 1 | [robot plotter] 2 | body_color: #9BC2F4 3 | body_outline_width: 8 4 | leg_color: #82B4F4 5 | leg_width: 10 6 | joint_size: 8 7 | head_color: red 8 | head_size: 10 9 | cog_color: gray 10 | cog_width: 6 11 | [axis] 12 | color_x: red 13 | color_y: green 14 | color_z: blue 15 | w_origin_color: black 16 | r_origin_color: orange 17 | origin_size: 6 18 | axis_size: 2 19 | [ground] 20 | size: 60 21 | color: gray 22 | -------------------------------------------------------------------------------- /ui.py: -------------------------------------------------------------------------------- 1 | import configparser 2 | 3 | import dash 4 | from dash import html, dcc 5 | import dash_bootstrap_components as dbc 6 | from dash import html, dcc, Input, Output, callback, State 7 | import plotly.graph_objects as go 8 | 9 | from constant import * 10 | from models import Hexapod 11 | 12 | # dimension control 13 | def make_slider(range, id, value, step=1, updatemode='drag'): 14 | return dcc.Slider(range[0], range[1], step, 15 | value=value, 16 | id=id, marks=None, 17 | updatemode = updatemode, 18 | tooltip={"placement": "right", "always_visible": True}) 19 | 20 | dim_ctl_ids = ['Front', 'Middle', 'Side', 'coxa', 'Femur', 'Tibia'] 21 | dim_ctl_labels = [html.Label(id) for id in dim_ctl_ids] 22 | values = DEFAULT_DIMSIONS + DEFAULT_LEG_LENGTH 23 | dim_ctl_sliders = [make_slider([1, 20], id, v) for id, v in zip(dim_ctl_ids, values)] 24 | dim_ctl_widgets = [dbc.Row(dbc.Col(html.Label(dcc.Markdown(f"**Dimension Setting**")), width=12), justify='center')] 25 | for l, s in zip(dim_ctl_labels, dim_ctl_sliders): 26 | dim_ctl_widgets.append(dbc.Row([dbc.Col(l, width=2, align='start'), 27 | dbc.Col(s, width=10, align='start', className="g-0")], 28 | justify='center')) 29 | dim_ctl_widgets.append(dbc.Row(dbc.Col(dbc.Button("Reset Dimension", outline=True, color="primary", className="me-1", id='reset-dim'), width=12), align='center',className="mt-3",)) 30 | dim_ctl_widgets.append(dbc.Row(dbc.Col(dbc.Button("Reset Poses", outline=True, color="primary", className="me-1", id='reset-pose'), width=12), align='center',className="mt-3")) 31 | dim_ctl_widgets.append(dbc.Row(dbc.Col(dbc.Button("Reset 3D View", outline=True, color="primary", className="me-1", id='reset-view'), width=12), align='center',className="mt-3")) 32 | 33 | # leg patterns 34 | leg_ctl_ids = ['alpha', 'beta', 'gamma'] 35 | leg_labels = [r'$\alpha$ (coxa-zaxis)', r'$\beta$ (femur-xaxis)', r'$\gamma$ (tibia-xaxis)'] 36 | leg_ctl_labels = [html.Div(dcc.Markdown(id, mathjax=True)) for id in leg_labels] 37 | leg_ctl_sliders = [make_slider([-180, 180], id, 0) for id in leg_ctl_ids] 38 | widgets = [html.Label(dcc.Markdown("*Legs share the same pose*"))] 39 | for l, s in zip(leg_ctl_labels, leg_ctl_sliders): 40 | widgets.append(l) 41 | widgets.append(s) 42 | 43 | leg_ctl_widgets = dbc.Card( 44 | dbc.CardBody( 45 | widgets 46 | ), 47 | className="mt-3", 48 | ) 49 | 50 | # forward kinematics 51 | fk_leg_labels = [] 52 | for v in LEG_ID_NAMES.values(): 53 | fk_leg_labels.append(dbc.Col(html.Label(dcc.Markdown(f"{v}")), width=2, align='center')) 54 | 55 | fk_sliders = [] 56 | fk_leg_ctl_ids = ['-fk-alpha', '-fk-beta', '-fk-gamma'] 57 | fk_leg_seg_labels = [r'$\alpha$', r'$\beta$', r'$\gamma$'] 58 | fk_slider_ids = [] 59 | for id, l in zip(fk_leg_ctl_ids, fk_leg_seg_labels): 60 | l_s = [] 61 | for v in LEG_ID_NAMES.values(): 62 | # l_s.append(dbc.Col(html.Div(dcc.Markdown(l, mathjax=True)), width=1)) 63 | s = make_slider([-180, 180], v + id, 0) 64 | fk_slider_ids.append(v + id) 65 | l_s.append(dbc.Col([html.Label(dcc.Markdown(l, mathjax=True)), s], width=2, align='center')) 66 | fk_sliders.append(dbc.Row(l_s)) 67 | 68 | fk_widgets = [dbc.Row(fk_leg_labels, align='center')] + fk_sliders 69 | fk_ctl_widgets = dbc.Card( 70 | dbc.CardBody( 71 | fk_widgets 72 | ), 73 | className="mt-3", 74 | ) 75 | 76 | # inverse kinematics 77 | ik_t_slider = [] 78 | ik_r_slider = [] 79 | for axis in AXIS_INDEX.keys(): 80 | ts = make_slider([-1, 1], 'IK-T'+axis, 0, step=0.01) 81 | ik_t_slider.append(dbc.Col([html.Label(dcc.Markdown('T'+axis, mathjax=True)), ts], width=4)) 82 | rs = make_slider([-30, 30], 'IK-R'+axis, 0, step=0.1) 83 | ik_r_slider.append(dbc.Col([html.Label(dcc.Markdown('R'+axis, mathjax=True)), rs], width=4)) 84 | 85 | ik_stance_slider = [ 86 | dbc.Col([html.Label(dcc.Markdown('Hip Stance', mathjax=True)), make_slider([-60, 60], 'IK-Hip Stance', 0, step=1)], width=6), 87 | dbc.Col([html.Label(dcc.Markdown('Leg Stance', mathjax=True)), make_slider([-60, 60], 'IK-Leg Stance', 0, step=1)], width=6) 88 | ] 89 | 90 | ik_ctl_widgets = dbc.Card( 91 | dbc.CardBody( 92 | [dbc.Row(ik_t_slider), dbc.Row(ik_r_slider), dbc.Row(ik_stance_slider)] 93 | ), 94 | className="mt-3", 95 | ) 96 | 97 | # walking gait 98 | gait_timer = dcc.Interval(id='walking-timer', 99 | interval=20, # in milliseconds 100 | n_intervals=0, 101 | max_intervals=0 102 | ) 103 | gait_play_bt = dbc.Button("Play", outline=True, color="primary", className="me-1", id='gait-play') 104 | gait_pause_bt = dbc.Button("Pause", outline=True, color="primary", className="me-1", id='gait-pause') 105 | gait_step_bt = dbc.Button(">>Step", outline=True, color="primary", className="me-1", id='gait-step') 106 | 107 | gait_ck = dcc.Checklist( 108 | [ { 109 | "label": html.Div(['Tripod'], style={'color': 'LightGreen', 'font-size': 20}), 110 | "value": 'is_tripod', 111 | }, 112 | { 113 | "label": html.Div(['Forward'], style={'color': 'Gold', 'font-size': 20}), 114 | "value": 'is_forward', 115 | }, 116 | { 117 | "label": html.Div(['Rotate'], style={'color': 'MediumTurqoise', 'font-size': 20}), 118 | "value": 'is_rotate', 119 | }, 120 | ], 121 | value=['is_tripod', 'is_forward'], 122 | labelStyle={"display": "flex", "align-items": "center"}, 123 | id='gait-ck' 124 | ) 125 | 126 | lift_swing_slider = make_slider([10,40], id='LiftSwing', value=20, step=1, updatemode='mouseup') 127 | hip_swing_slider = make_slider([10,40], id='HipSwing', value=12, step=1, updatemode='mouseup') 128 | step_swing_slider = make_slider([5,20], id='GaitStep', value=10, step=1, updatemode='mouseup') 129 | gait_speed_slider = make_slider([5,20], id='GaitSpeed', value=10, step=1, updatemode='mouseup') 130 | gait_sliders = [lift_swing_slider, hip_swing_slider, step_swing_slider, gait_speed_slider] 131 | gait_slider_label = [] 132 | for s in gait_sliders: 133 | gait_slider_label.append(html.Label(s.id)) 134 | gait_widget = dbc.Card( 135 | dbc.CardBody( 136 | dbc.Row([ 137 | dbc.Col(gait_slider_label, width=1), 138 | dbc.Col(gait_sliders, width=4), 139 | dbc.Col(gait_ck, width=3), 140 | dbc.Col([gait_play_bt, gait_pause_bt, gait_step_bt], width=3), 141 | gait_timer 142 | ]) 143 | ), 144 | className="mt-3", 145 | ) 146 | 147 | # configures 148 | conf = configparser.ConfigParser() 149 | conf.read('style.ini', encoding='utf-8') 150 | 151 | def draw_robot(robot:Hexapod): 152 | # generate data for hexapod plotting 153 | body_mesh = go.Mesh3d( 154 | x=[p.x for p in robot.body.vertices], 155 | y=[p.y for p in robot.body.vertices], 156 | z=[p.z for p in robot.body.vertices], 157 | color=conf["robot plotter"]['body_color'], 158 | name='robot-body-mesh', 159 | showlegend=False, 160 | opacity=0.7, 161 | i=[0,1,0,0], 162 | j=[1,2,3,4], 163 | k=[3,3,4,5], 164 | ) 165 | 166 | body_outline = go.Scatter3d( 167 | x=[p.x for p in robot.body.vertices] + [robot.body.vertices[0].x], 168 | y=[p.y for p in robot.body.vertices] + [robot.body.vertices[0].y], 169 | z=[p.z for p in robot.body.vertices] + [robot.body.vertices[0].z], 170 | name='robot-body_outline', 171 | marker=dict(color=conf["robot plotter"]['leg_color'], 172 | size=int(conf["robot plotter"]['joint_size'])), 173 | line=dict(width=int(conf["robot plotter"]['body_outline_width'])), 174 | showlegend=False 175 | ) 176 | 177 | head = go.Scatter3d( 178 | x=[robot.body.head.x], 179 | y=[robot.body.head.y], 180 | z=[robot.body.head.z], 181 | name='robot-head', 182 | marker=dict(color=conf["robot plotter"]['head_color'], 183 | size=int(conf["robot plotter"]['head_size'])), 184 | ) 185 | 186 | 187 | graph_data = [body_mesh, body_outline, head] 188 | 189 | for i in range(6): 190 | leg = go.Scatter3d( 191 | x=[p.x for p in robot.legs[i].points_global], 192 | y=[p.y for p in robot.legs[i].points_global], 193 | z=[p.z for p in robot.legs[i].points_global], 194 | name='robot-leg-' + str(i), 195 | marker=dict(color=conf["robot plotter"]['leg_color'], 196 | size=int(conf["robot plotter"]['joint_size'])), 197 | line=dict(width=int(conf["robot plotter"]['leg_width'])), 198 | showlegend=False 199 | ) 200 | graph_data.append(leg) 201 | 202 | support_mesh = go.Mesh3d( 203 | x=[p.x for p in robot.ground_contact_points.values()], 204 | y=[p.y for p in robot.ground_contact_points.values()], 205 | z=[p.z-0.01 for p in robot.ground_contact_points.values()], 206 | color=conf["robot plotter"]['body_color'], 207 | name='support-mesh', 208 | showlegend=False, 209 | opacity=0.2, 210 | ) 211 | graph_data.append(support_mesh) 212 | 213 | robot_axis = [ 214 | go.Scatter3d( 215 | x=[robot.body.cog.x], 216 | y=[robot.body.cog.y], 217 | z=[robot.body.cog.z], 218 | name='robot-axis', 219 | marker=dict(color=conf["axis"]['r_origin_color'], 220 | size=int(conf["axis"]['origin_size'])) 221 | ), 222 | # x-axis 223 | go.Scatter3d( 224 | x = [robot.body.cog.x, robot.x_axis.x], 225 | y = [robot.body.cog.y, robot.x_axis.y], 226 | z = [robot.body.cog.z, robot.x_axis.z], 227 | mode='lines', 228 | line=dict(color=conf["axis"]['color_x']), 229 | showlegend=False 230 | ), 231 | go.Scatter3d( 232 | x = [robot.body.cog.x, robot.y_axis.x], 233 | y = [robot.body.cog.y, robot.y_axis.y], 234 | z = [robot.body.cog.z, robot.y_axis.z], 235 | mode='lines', 236 | line=dict(color=conf["axis"]['color_y']), 237 | showlegend=False 238 | ), 239 | go.Scatter3d( 240 | x = [robot.body.cog.x, robot.z_axis.x], 241 | y = [robot.body.cog.y, robot.z_axis.y], 242 | z = [robot.body.cog.z, robot.z_axis.z], 243 | mode='lines', 244 | line=dict(color=conf["axis"]['color_z']), 245 | showlegend=False 246 | ) 247 | ] 248 | 249 | world_axis = [ 250 | go.Scatter3d( 251 | x=[0], 252 | y=[0], 253 | z=[0], 254 | name='world-axis', 255 | marker=dict(color=conf["axis"]['w_origin_color'], 256 | size=int(conf["axis"]['origin_size'])) 257 | ), 258 | # x-axis 259 | go.Scatter3d( 260 | x = [0, conf["axis"]['axis_size']], 261 | y = [0, 0], 262 | z = [0, 0], 263 | mode='lines', 264 | line=dict(color=conf["axis"]['color_x']), 265 | showlegend=False 266 | ), 267 | # y-axis 268 | go.Scatter3d( 269 | x = [0, 0], 270 | y = [0, conf["axis"]['axis_size']], 271 | z = [0, 0], 272 | mode='lines', 273 | line=dict(color=conf["axis"]['color_y']), 274 | showlegend=False 275 | ), 276 | # z-axis 277 | go.Scatter3d( 278 | x = [0, 0], 279 | y = [0, 0], 280 | z = [0, conf["axis"]['axis_size']], 281 | mode='lines', 282 | line=dict(color=conf["axis"]['color_z']), 283 | showlegend=False 284 | ) 285 | ] 286 | graph_data += robot_axis + world_axis 287 | 288 | # ground 289 | s = int(conf['ground']['size']) 290 | ground_mesh = go.Mesh3d( 291 | x=[s/2, -s/2, -s/2, s/2], 292 | y=[s/2, s/2, -s/2, -s/2], 293 | z=[0, 0, 0, 0], 294 | color=conf['ground']['color'], 295 | name='ground', 296 | showlegend=False, 297 | opacity=0.2, 298 | showscale=False 299 | 300 | ) 301 | 302 | graph_data += [ground_mesh] 303 | return graph_data 304 | 305 | 306 | def update_robot_graph(fig, robot:Hexapod): 307 | # body mesh 308 | fig["data"][0]['x'] = [p.x for p in robot.body.vertices] 309 | fig["data"][0]['y'] = [p.y for p in robot.body.vertices] 310 | fig["data"][0]['z'] = [p.z for p in robot.body.vertices] 311 | 312 | # body outline 313 | fig["data"][1]['x'] = [p.x for p in robot.body.vertices] + [robot.body.vertices[0].x] 314 | fig["data"][1]['y'] = [p.y for p in robot.body.vertices] + [robot.body.vertices[0].y] 315 | fig["data"][1]['z'] = [p.z for p in robot.body.vertices] + [robot.body.vertices[0].z] 316 | 317 | # head 318 | fig["data"][2]['x'] = [robot.body.head.x] 319 | fig["data"][2]['y'] = [robot.body.head.y] 320 | fig["data"][2]['z'] = [robot.body.head.z] 321 | 322 | # robot legs 323 | for i in range(6): 324 | fig["data"][i+3]['x'] = [p.x for p in robot.legs[i].points_global] 325 | fig["data"][i+3]['y'] = [p.y for p in robot.legs[i].points_global] 326 | fig["data"][i+3]['z'] = [p.z for p in robot.legs[i].points_global] 327 | 328 | # support mesh 329 | fig["data"][9]['x'] = [p.x for p in robot.ground_contact_points.values()] 330 | fig["data"][9]['y'] = [p.y for p in robot.ground_contact_points.values()] 331 | fig["data"][9]['z'] = [p.z-0.01 for p in robot.ground_contact_points.values()] 332 | 333 | # robot axis 334 | fig["data"][10]['x'] = [robot.body.cog.x] 335 | fig["data"][10]['y'] = [robot.body.cog.y] 336 | fig["data"][10]['z'] = [robot.body.cog.z] 337 | 338 | fig["data"][11]['x'] = [robot.body.cog.x, robot.x_axis.x] 339 | fig["data"][11]['y'] = [robot.body.cog.y, robot.x_axis.y] 340 | fig["data"][11]['z'] = [robot.body.cog.z, robot.x_axis.z] 341 | 342 | fig["data"][12]['x'] = [robot.body.cog.x, robot.y_axis.x] 343 | fig["data"][12]['y'] = [robot.body.cog.y, robot.y_axis.y] 344 | fig["data"][12]['z'] = [robot.body.cog.z, robot.y_axis.z] 345 | 346 | fig["data"][13]['x'] = [robot.body.cog.x, robot.z_axis.x] 347 | fig["data"][13]['y'] = [robot.body.cog.y, robot.z_axis.y] 348 | fig["data"][13]['z'] = [robot.body.cog.z, robot.z_axis.z] 349 | 350 | return fig 351 | 352 | def play_robot_walking(fig, robot:Hexapod, t): 353 | robot.set_pose_from_walking_sequence(t) 354 | return update_robot_graph(fig, robot) 355 | 356 | 357 | # robot instance 358 | robot = Hexapod() 359 | graph_data = draw_robot(robot) 360 | 361 | camera = dict( 362 | up=dict(x=0, y=0, z=1), 363 | center=dict(x=0, y=0, z=0), 364 | eye=dict(x=0, y=0.5, z=0.4) 365 | ) 366 | 367 | fig = go.Figure(data=graph_data) 368 | fig.update_layout(height=600, 369 | uirevision=True, 370 | scene={ 371 | "camera":camera, 372 | "aspectmode": "manual", 373 | "aspectratio": {"x": 1, "y": 1, "z": 1}, 374 | 'xaxis':{"nticks":1, "backgroundcolor":"white", "range": [-20, 20], "tickfont":dict(color="white")}, 375 | 'yaxis':{"nticks":1, "backgroundcolor":"white", "range": [-20, 20], "tickfont":dict(color="white")}, 376 | 'zaxis':{"nticks":1, "backgroundcolor":"white", "range": [-20, 20], "tickfont":dict(color="white")}, 377 | }) 378 | # buttons 379 | @callback( 380 | Output('Front', 'value'), 381 | Output('Middle', 'value'), 382 | Output('Side', 'value'), 383 | Output('coxa', 'value'), 384 | Output('Femur', 'value'), 385 | Output('Tibia', 'value'), 386 | Input('reset-dim', 'n_clicks'), 387 | prevent_initial_call=True 388 | ) 389 | def reset_robot_dimension(btn): 390 | return DEFAULT_DIMSIONS + DEFAULT_LEG_LENGTH 391 | 392 | 393 | @callback( 394 | Output('graph', 'figure', allow_duplicate=True), 395 | Input('reset-view', 'n_clicks'), 396 | prevent_initial_call=True 397 | ) 398 | def reset_camera_view(btn): 399 | fig['layout']['uirevision'] = False 400 | fig['layout']['scene']['camera'] = camera 401 | # fig['layout']['uirevision'] = True 402 | return fig 403 | 404 | 405 | # robot control 406 | # dimension 407 | @callback( 408 | Output('graph', 'figure', allow_duplicate=True), 409 | Input('Front', 'value'), 410 | Input('Middle', 'value'), 411 | Input('Side', 'value'), 412 | Input('coxa', 'value'), 413 | Input('Femur', 'value'), 414 | Input('Tibia', 'value'), 415 | prevent_initial_call=True 416 | ) 417 | def change_robot_dimension(f, m, s, coxa, femur, tibia): 418 | if robot.update_dimensions([f, m, s, coxa, femur, tibia]): 419 | update_robot_graph(fig, robot) 420 | return fig 421 | # leg patterns 422 | @callback( 423 | Output('graph', 'figure'), 424 | Input('alpha', 'value'), 425 | Input('beta', 'value'), 426 | Input('gamma', 'value'), 427 | prevent_initial_call=True 428 | ) 429 | def change_robot_leg_pattern(a,b,c): 430 | if robot.update_leg_pattern([a, b, c]): 431 | update_robot_graph(fig, robot) 432 | return fig 433 | 434 | # forward kinematics 435 | html_e = [Output('graph', 'figure', allow_duplicate=True)] 436 | for eid in fk_slider_ids: 437 | html_e.append(Input(eid, "value")) 438 | @callback( 439 | *html_e, prevent_initial_call=True 440 | ) 441 | def forward_kinematics(*args): 442 | poses = {} 443 | for leg, leg_id in LEG_NAMES_ID.items(): 444 | a = {} 445 | for seg, seg_id in LEG_SEG_NAMES_ID.items(): 446 | a[seg] = args[int(leg_id + seg_id*6)] 447 | poses[leg] = a 448 | if robot.update_leg_pose(poses): 449 | update_robot_graph(fig, robot) 450 | return fig 451 | 452 | # inverse kinematics 453 | @callback( 454 | Output('graph', 'figure', allow_duplicate=True), 455 | Input('IK-RX', 'value'), 456 | Input('IK-RY', 'value'), 457 | Input('IK-RZ', 'value'), 458 | Input('IK-TX', 'value'), 459 | Input('IK-TY', 'value'), 460 | Input('IK-TZ', 'value'), 461 | prevent_initial_call=True 462 | ) 463 | def inverse_kinematics(rx, ry, rz, tx, ty, tz): 464 | tx *= robot.body.f 465 | ty *= robot.body.s 466 | tz *= robot.legs[0].lengths[-1] 467 | robot.solve_ik([rx, ry, rz], [tx, ty, tz]) 468 | update_robot_graph(fig, robot) 469 | return fig 470 | 471 | # gait 472 | @callback( 473 | Output('graph', 'figure', allow_duplicate=True), 474 | Input('walking-timer', 'n_intervals'), 475 | prevent_initial_call=True 476 | ) 477 | def walking(n): 478 | play_robot_walking(fig, robot, n%len(robot.walking_sequence[0]['coxa'])) 479 | return fig 480 | 481 | @callback( 482 | Output('walking-timer', 'max_intervals', allow_duplicate=True), 483 | Input('gait-play', 'n_clicks'), 484 | prevent_initial_call=True 485 | ) 486 | def play_gait(n): 487 | return -1 488 | 489 | @callback( 490 | Output('walking-timer', 'max_intervals'), 491 | Input('gait-pause', 'n_clicks'), 492 | prevent_initial_call=True 493 | ) 494 | def pause_gait(n): 495 | return 0 496 | 497 | @callback( 498 | Output('walking-timer', 'n_intervals'), 499 | Input('gait-step', 'n_clicks'), 500 | State('walking-timer', 'n_intervals'), 501 | prevent_initial_call=True 502 | ) 503 | def pause_gait(s, n): 504 | return s + 1 505 | 506 | @callback( 507 | Output('walking-timer', 'n_intervals', allow_duplicate=True), 508 | Input('LiftSwing', 'value'), 509 | Input('HipSwing', 'value'), 510 | Input('GaitStep', 'value'), 511 | Input('GaitSpeed', 'value'), 512 | Input('gait-ck', 'value'), 513 | prevent_initial_call=True 514 | ) 515 | def update_gait_parameters(ls, hs, st, sp, ck): 516 | para = {} 517 | para['HipSwing'] = hs 518 | para['LiftSwing'] = ls 519 | para['StepNum'] = st 520 | para['Speed'] = sp 521 | para['Gait'] = 'Tripod' if 'is_tripod' in ck else 'Ripple' 522 | para['Direction'] = 1 if 'is_forward' in ck else -1 523 | para['Rotation'] = 1 if 'is_rotate' in ck else 0 524 | robot.generate_walking_sequence(para) 525 | print(para) 526 | return 0 527 | 528 | if __name__ == "__main__": 529 | app = dash.Dash( 530 | external_stylesheets=[dbc.themes.BOOTSTRAP] 531 | ) 532 | 533 | app.layout = html.Div( 534 | dbc.Container( 535 | [ 536 | dbc.Row([ 537 | dbc.Col(dim_ctl_widgets, width={"size": 3, "offset": 0}, align='center'), 538 | dbc.Col( 539 | html.Div(dcc.Graph(figure=fig, id='graph')), 540 | width={"size": 9, "offset": 0}, 541 | ) 542 | ], align='center', justify='center'), 543 | 544 | dbc.Row([ 545 | dcc.Tabs(id="page-tabs", value='leg-patterns', 546 | children=[ 547 | dcc.Tab(children=leg_ctl_widgets, label='Leg Pattern', value='leg-patterns'), 548 | dcc.Tab(children=fk_ctl_widgets,label='Forward Kinematics', value='FK'), 549 | dcc.Tab(children=ik_ctl_widgets,label='Inverse Kinematics', value='IK'), 550 | dcc.Tab(children=gait_widget, label='Walking Gaits', value='Walk'), 551 | ]) 552 | ]), 553 | ] 554 | ) 555 | ) 556 | 557 | app.run_server(debug=True) --------------------------------------------------------------------------------