dorian.veloso
/
MachineVision_Lab2_VELOSO_PHERWANI
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Adding code before deadline

This commit is contained in:
Dorian VELOSO 2025-04-20 22:51:14 +02:00
parent 6c7bb3fe17
commit 7e3405a437
29 changed files with 167796 additions and 190 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
DynamixelSDK Submodule

@ -0,0 +1 @@
Subproject commit be0a091afafb923c25582696ebf0f87fbb32b428

317
LICENSE
View File

@ -1,295 +1,232 @@
ADAPTIVE PUBLIC LICENSE GNU GENERAL PUBLIC LICENSE
Version 1.0 Version 3, 29 June 2007
THE LICENSED WORK IS PROVIDED UNDER THE TERMS OF THIS ADAPTIVE PUBLIC LICENSE ("LICENSE"). ANY USE, REPRODUCTION OR DISTRIBUTION OF THE LICENSED WORK CONSTITUTES RECIPIENT'S ACCEPTANCE OF THIS LICENSE AND ITS TERMS, WHETHER OR NOT SUCH RECIPIENT READS THE TERMS OF THIS LICENSE. "LICENSED WORK" AND "RECIPIENT" ARE DEFINED BELOW. Copyright © 2007 Free Software Foundation, Inc. <https://fsf.org/>
IMPORTANT NOTE: This License is "adaptive", and the generic version or another version of an Adaptive Public License should not be relied upon to determine your rights and obligations under this License. You must read the specific Adaptive Public License that you receive with the Licensed Work, as certain terms are defined at the outset by the Initial Contributor. Everyone is permitted to copy and distribute verbatim copies of this license document, but changing it is not allowed.
See Section 2.2 below, Exhibit A attached, and any Suppfile.txt accompanying this License to determine the specific adaptive features applicable to this License. For example, without limiting the foregoing, (a) for selected choice of law and jurisdiction see Part 3 of Exhibit A; (b) for the selected definition of Third Party see Part 4 of Exhibit A; and (c) for selected patent licensing terms (if any) see Section 2.2 below and Part 6 of Exhibit A. Preamble
1. DEFINITIONS. The GNU General Public License is a free, copyleft license for software and other kinds of works.
1.1. "CONTRIBUTION" means: The licenses for most software and other practical works are designed to take away your freedom to share and change the works. By contrast, the GNU General Public License is intended to guarantee your freedom to share and change all versions of a program--to make sure it remains free software for all its users. We, the Free Software Foundation, use the GNU General Public License for most of our software; it applies also to any other work released this way by its authors. You can apply it to your programs, too.
(a) In the case of the Initial Contributor, the Initial Work distributed under this License by the Initial Contributor; and When we speak of free software, we are referring to freedom, not price. Our General Public Licenses are designed to make sure that you have the freedom to distribute copies of free software (and charge for them if you wish), that you receive source code or can get it if you want it, that you can change the software or use pieces of it in new free programs, and that you know you can do these things.
(b) In the case of each Subsequent Contributor, the Subsequent Work originating from and distributed by such Subsequent Contributor.
1.2. "DESIGNATED WEB SITE" means the web site having the URL identified in Part 1 of Exhibit A, which URL may be changed by the Initial Contributor by posting on the current Designated Web Site the new URL for at least sixty (60) days. To protect your rights, we need to prevent others from denying you these rights or asking you to surrender the rights. Therefore, you have certain responsibilities if you distribute copies of the software, or if you modify it: responsibilities to respect the freedom of others.
1.3. "DISTRIBUTOR" means any Person that distributes the Licensed Work or any portion thereof to at least one Third Party. For example, if you distribute copies of such a program, whether gratis or for a fee, you must pass on to the recipients the same freedoms that you received. You must make sure that they, too, receive or can get the source code. And you must show them these terms so they know their rights.
1.4. "ELECTRONIC DISTRIBUTION MECHANISM" means any mechanism generally accepted in the software development community for the electronic transfer of data. Developers that use the GNU GPL protect your rights with two steps: (1) assert copyright on the software, and (2) offer you this License giving you legal permission to copy, distribute and/or modify it.
1.5. "EXECUTABLE" means the Licensed Work in any form other than Source Code. For the developers' and authors' protection, the GPL clearly explains that there is no warranty for this free software. For both users' and authors' sake, the GPL requires that modified versions be marked as changed, so that their problems will not be attributed erroneously to authors of previous versions.
1.6. "GOVERNING JURISDICTION" means the state, province or other legal jurisdiction identified in Part 3 of Exhibit A. Some devices are designed to deny users access to install or run modified versions of the software inside them, although the manufacturer can do so. This is fundamentally incompatible with the aim of protecting users' freedom to change the software. The systematic pattern of such abuse occurs in the area of products for individuals to use, which is precisely where it is most unacceptable. Therefore, we have designed this version of the GPL to prohibit the practice for those products. If such problems arise substantially in other domains, we stand ready to extend this provision to those domains in future versions of the GPL, as needed to protect the freedom of users.
1.7. "INDEPENDENT MODULE" means a separate module of software and/or data that is not a derivative work of or copied from the Licensed Work or any portion thereof. In addition, a module does not qualify as an Independent Module but instead forms part of the Licensed Work if the module: (a) is embedded in the Licensed Work; (b) is included by reference in the Licensed Work other than by a function call or a class reference; or (c) must be included or contained, in whole or in part, within a file directory or subdirectory actually containing files making up the Licensed Work. Finally, every program is threatened constantly by software patents. States should not allow patents to restrict development and use of software on general-purpose computers, but in those that do, we wish to avoid the special danger that patents applied to a free program could make it effectively proprietary. To prevent this, the GPL assures that patents cannot be used to render the program non-free.
1.8. "INITIAL CONTRIBUTOR" means the Person or entity identified as the Initial Contributor in the notice required by Part 1 of Exhibit A. The precise terms and conditions for copying, distribution and modification follow.
1.9. "INITIAL WORK" means the initial Source Code, object code (if any) and documentation for the computer program identified in Part 2 of Exhibit A, as such Source Code, object code and documentation is distributed under this License by the Initial Contributor. TERMS AND CONDITIONS
1.10. "LARGER WORK" means a work that combines the Licensed Work or portions thereof with code not governed by this License. 0. Definitions.
1.11. "LICENSED WORK" means the Initial Work and/or any Subsequent Work, in each case including portions thereof. “This License” refers to version 3 of the GNU General Public License.
1.12. "LICENSE NOTICE" has the meaning assigned in Part 5 of Exhibit A. “Copyright” also means copyright-like laws that apply to other kinds of works, such as semiconductor masks.
1.13. "MODIFICATION" or "MODIFICATIONS" means any change to and/or addition to the Licensed Work. “The Program” refers to any copyrightable work licensed under this License. Each licensee is addressed as “you”. “Licensees” and “recipients” may be individuals or organizations.
1.14. "PERSON" means an individual or other legal entity, including a corporation, partnership or other body. To “modify” a work means to copy from or adapt all or part of the work in a fashion requiring copyright permission, other than the making of an exact copy. The resulting work is called a “modified version” of the earlier work or a work “based on” the earlier work.
1.15. "RECIPIENT" means any Person who receives or obtains the Licensed Work under this License (by way of example, without limiting the foregoing, any Subsequent Contributor or Distributor). A “covered work” means either the unmodified Program or a work based on the Program.
1.16. "SOURCE CODE" means the source code for a computer program, including the source code for all modules and components of the computer program, plus any associated interface definition files, and scripts used to control compilation and installation of an executable. To “propagate” a work means to do anything with it that, without permission, would make you directly or secondarily liable for infringement under applicable copyright law, except executing it on a computer or modifying a private copy. Propagation includes copying, distribution (with or without modification), making available to the public, and in some countries other activities as well.
1.17. "SUBSEQUENT CONTRIBUTOR" means any Person that makes or contributes to the making of any Subsequent Work and that distributes that Subsequent Work to at least one Third Party. To “convey” a work means any kind of propagation that enables other parties to make or receive copies. Mere interaction with a user through a computer network, with no transfer of a copy, is not conveying.
1.18. "SUBSEQUENT WORK" means a work that has resulted or arises from changes to and/or additions to: An interactive user interface displays “Appropriate Legal Notices” to the extent that it includes a convenient and prominently visible feature that (1) displays an appropriate copyright notice, and (2) tells the user that there is no warranty for the work (except to the extent that warranties are provided), that licensees may convey the work under this License, and how to view a copy of this License. If the interface presents a list of user commands or options, such as a menu, a prominent item in the list meets this criterion.
(a) the Initial Work; 1. Source Code.
(b) any other Subsequent Work; or The “source code” for a work means the preferred form of the work for making modifications to it. “Object code” means any non-source form of a work.
(c) to any combination of the Initial Work and any such other Subsequent Work;
where such changes and/or additions originate from a Subsequent Contributor. A Subsequent Work will "originate" from a Subsequent Contributor if the Subsequent Work was a result of efforts by such Subsequent Contributor (or anyone acting on such Subsequent Contributor's behalf, such as, a contractor or other entity that is engaged by or under the direction of the Subsequent Contributor). For greater certainty, a Subsequent Work expressly excludes and shall not capture within its meaning any Independent Module.
1.19. "SUPPLEMENT FILE" means a file distributed with the Licensed Work having a file name "suppfile.txt". A “Standard Interface” means an interface that either is an official standard defined by a recognized standards body, or, in the case of interfaces specified for a particular programming language, one that is widely used among developers working in that language.
1.20. "THIRD PARTY" has the meaning assigned in Part 4 of Exhibit A. The “System Libraries” of an executable work include anything, other than the work as a whole, that (a) is included in the normal form of packaging a Major Component, but which is not part of that Major Component, and (b) serves only to enable use of the work with that Major Component, or to implement a Standard Interface for which an implementation is available to the public in source code form. A “Major Component”, in this context, means a major essential component (kernel, window system, and so on) of the specific operating system (if any) on which the executable work runs, or a compiler used to produce the work, or an object code interpreter used to run it.
2. LICENSE. The “Corresponding Source” for a work in object code form means all the source code needed to generate, install, and (for an executable work) run the object code and to modify the work, including scripts to control those activities. However, it does not include the work's System Libraries, or general-purpose tools or generally available free programs which are used unmodified in performing those activities but which are not part of the work. For example, Corresponding Source includes interface definition files associated with source files for the work, and the source code for shared libraries and dynamically linked subprograms that the work is specifically designed to require, such as by intimate data communication or control flow between those subprograms and other parts of the work.
2.1. COPYRIGHT LICENSE FROM INITIAL AND SUBSEQUENT CONTRIBUTORS. The Corresponding Source need not include anything that users can regenerate automatically from other parts of the Corresponding Source.
(a) Subject to the terms of this License, the Initial Contributor hereby grants each Recipient a world-wide, royalty-free, non-exclusive copyright license to: The Corresponding Source for a work in source code form is that same work.
(i) reproduce, prepare derivative works of, publicly display, publicly perform, distribute and sublicense the Initial Work; and 2. Basic Permissions.
(ii) reproduce, publicly display, publicly perform, distribute, and sublicense any derivative works (if any) prepared by Recipient; All rights granted under this License are granted for the term of copyright on the Program, and are irrevocable provided the stated conditions are met. This License explicitly affirms your unlimited permission to run the unmodified Program. The output from running a covered work is covered by this License only if the output, given its content, constitutes a covered work. This License acknowledges your rights of fair use or other equivalent, as provided by copyright law.
in Source Code and Executable form, either with other Modifications, on an unmodified basis, or as part of a Larger Work.
(b) Subject to the terms of this License, each Subsequent Contributor hereby grants each Recipient a world-wide, royalty-free, non-exclusive copyright license to: You may make, run and propagate covered works that you do not convey, without conditions so long as your license otherwise remains in force. You may convey covered works to others for the sole purpose of having them make modifications exclusively for you, or provide you with facilities for running those works, provided that you comply with the terms of this License in conveying all material for which you do not control copyright. Those thus making or running the covered works for you must do so exclusively on your behalf, under your direction and control, on terms that prohibit them from making any copies of your copyrighted material outside their relationship with you.
(i) reproduce, prepare derivative works of, publicly display, publicly perform, distribute and sublicense the Subsequent Work of such Subsequent Contributor; and Conveying under any other circumstances is permitted solely under the conditions stated below. Sublicensing is not allowed; section 10 makes it unnecessary.
(ii) reproduce, publicly display, publicly perform, distribute, and sublicense any derivative works (if any) prepared by Recipient;
in Source Code and Executable form, either with other Modifications, on an unmodified basis, or as part of a Larger Work.
2.2. PATENT LICENSE FROM INITIAL AND SUBSEQUENT CONTRIBUTORS. 3. Protecting Users' Legal Rights From Anti-Circumvention Law.
No covered work shall be deemed part of an effective technological measure under any applicable law fulfilling obligations under article 11 of the WIPO copyright treaty adopted on 20 December 1996, or similar laws prohibiting or restricting circumvention of such measures.
(a) This License does not include or grant any patent license whatsoever from the Initial Contributor, Subsequent Contributor, or any Distributor unless, at the time the Initial Work is first distributed or made available under this License (as the case may be), the Initial Contributor has selected pursuant to Part 6 of Exhibit A the patent terms in paragraphs A, B, C, D and E from Part 6 of Exhibit A. If this is not done then the Initial Work and any other Subsequent Work is made available under the License without any patent license (the "PATENTS-EXCLUDED LICENSE"). When you convey a covered work, you waive any legal power to forbid circumvention of technological measures to the extent such circumvention is effected by exercising rights under this License with respect to the covered work, and you disclaim any intention to limit operation or modification of the work as a means of enforcing, against the work's users, your or third parties' legal rights to forbid circumvention of technological measures.
(b) However, the Initial Contributor may subsequently distribute or make available (as the case may be) future copies of: (1) the Initial Work; or (2) any Licensed Work distributed by the Initial Contributor which includes the Initial Work (or any portion thereof) and/or any Modification made by the Initial Contributor; available under a License which includes a patent license (the "PATENTS-INCLUDED LICENSE") by selecting pursuant to Part 6 of Exhibit A the patent terms in paragraphs A, B, C, D and E from Part 6 of Exhibit A, when the Initial Contributor distributes or makes available (as the case may be) such future copies under this License.
(c) If any Recipient receives or obtains one or more copies of the Initial Work or any other portion of the Licensed Work under the Patents-Included License, then all licensing of such copies under this License shall include the terms in paragraphs A, B, C, D and E from Part 6 of Exhibit A and that Recipient shall not be able to rely upon the Patents-Excluded License for any such copies. However, all Recipients that receive one or more copies of the Initial Work or any other portion of the Licensed Work under a copy of the License which includes the Patents-Excluded License shall have no patent license with respect to such copies received under the Patents-Excluded License and availability and distribution of such copies, including Modifications made by such Recipient to such copies, shall be under a copy of the License without any patent license.
(d) Where a Recipient uses in combination or combines any copy of the Licensed Work (or portion thereof) licensed under a copy of the License having a Patents-Excluded License with any copy of the Licensed Work (or portion thereof) licensed under a copy of the License having a Patents-Included License, the combination (and any portion thereof) shall, from the first time such Recipient uses, makes available or distributes the combination (as the case may be), be subject to only the terms of the License having the Patents-Included License which shall include the terms in paragraphs A, B, C, D and E from Part 6 of Exhibit A.
2.3. ACKNOWLEDGEMENT AND DISCLAIMER. 4. Conveying Verbatim Copies.
Recipient understands and agrees that although Initial Contributor and each Subsequent Contributor grants the licenses to its Contributions set forth herein, no representation, warranty, guarantee or assurance is provided by any Initial Contributor, Subsequent Contributor, or Distributor that the Licensed Work does not infringe the patent or other intellectual property rights of any other entity. Initial Contributor, Subsequent Contributor, and each Distributor disclaims any liability to Recipient for claims brought by any other entity based on infringement of intellectual property rights or otherwise, in relation to the Licensed Works. As a condition to exercising the rights and licenses granted hereunder, each Recipient hereby assumes sole responsibility to secure any other intellectual property rights needed, if any. For example, without limiting the foregoing disclaimers, if a third party patent license is required to allow Recipient to distribute the Licensed Work, it is Recipient's responsibility to acquire that license before distributing the Licensed Work. You may convey verbatim copies of the Program's source code as you receive it, in any medium, provided that you conspicuously and appropriately publish on each copy an appropriate copyright notice; keep intact all notices stating that this License and any non-permissive terms added in accord with section 7 apply to the code; keep intact all notices of the absence of any warranty; and give all recipients a copy of this License along with the Program.
2.4. RESERVATION. You may charge any price or no price for each copy that you convey, and you may offer support or warranty protection for a fee.
Nothing in this License shall be deemed to grant any rights to trademarks, copyrights, patents, trade secrets or any other intellectual property of Initial Contributor, Subsequent Contributor, or Distributor except as expressly stated herein.
3. DISTRIBUTION OBLIGATIONS. 5. Conveying Modified Source Versions.
You may convey a work based on the Program, or the modifications to produce it from the Program, in the form of source code under the terms of section 4, provided that you also meet all of these conditions:
3.1. DISTRIBUTION GENERALLY. a) The work must carry prominent notices stating that you modified it, and giving a relevant date.
(a) A Subsequent Contributor shall make that Subsequent Contributor's Subsequent Work(s) available to the public via an Electronic Distribution Mechanism for a period of at least twelve (12) months. The aforesaid twelve (12) month period shall begin within a reasonable time after the creation of the Subsequent Work and no later than sixty (60) days after first distribution of that Subsequent Contributor's Subsequent Work. b) The work must carry prominent notices stating that it is released under this License and any conditions added under section 7. This requirement modifies the requirement in section 4 to “keep intact all notices”.
(b) All Distributors must distribute the Licensed Work in accordance with the terms of the License, and must include a copy of this License (including without limitation Exhibit A and the accompanying Supplement File) with each copy of the Licensed Work distributed. In particular, this License must be prominently distributed with the Licensed Work in a file called "license.txt." In addition, the License Notice in Part 5 of Exhibit A must be included at the beginning of all Source Code files, and viewable to a user in any executable such that the License Notice is reasonably brought to the attention of any party using the Licensed Work.
3.2. EXECUTABLE DISTRIBUTIONS OF THE LICENSED WORK. c) You must license the entire work, as a whole, under this License to anyone who comes into possession of a copy. This License will therefore apply, along with any applicable section 7 additional terms, to the whole of the work, and all its parts, regardless of how they are packaged. This License gives no permission to license the work in any other way, but it does not invalidate such permission if you have separately received it.
A Distributor may choose to distribute the Licensed Work, or any portion thereof, in Executable form (an "EXECUTABLE DISTRIBUTION") to any third party, under the terms of Section 2 of this License, provided the Executable Distribution is made available under and accompanied by a copy of this License, AND provided at least ONE of the following conditions is fulfilled:
(a) The Executable Distribution must be accompanied by the Source Code for the Licensed Work making up the Executable Distribution, and the Source Code must be distributed on the same media as the Executable Distribution or using an Electronic Distribution Mechanism; or d) If the work has interactive user interfaces, each must display Appropriate Legal Notices; however, if the Program has interactive interfaces that do not display Appropriate Legal Notices, your work need not make them do so.
(b) The Executable Distribution must be accompanied with a written offer, valid for at least thirty six (36) months, to give any third party under the terms of this License, for a charge no more than the cost of physically performing source distribution, a complete machine-readable copy of the Source Code for the Licensed Work making up the Executable Distribution, to be available and distributed using an Electronic Distribution Mechanism, and such Executable Distribution must remain available in Source Code form to any third party via the Electronic Distribution Mechanism (or any replacement Electronic Distribution Mechanism the particular Distributor may reasonably need to turn to as a substitute) for said at least thirty six (36) months.
For greater certainty, the above-noted requirements apply to any Licensed Work or portion thereof distributed to any third party in Executable form, whether such distribution is made alone, in combination with a Larger Work or Independent Modules, or in some other combination. A compilation of a covered work with other separate and independent works, which are not by their nature extensions of the covered work, and which are not combined with it such as to form a larger program, in or on a volume of a storage or distribution medium, is called an “aggregate” if the compilation and its resulting copyright are not used to limit the access or legal rights of the compilation's users beyond what the individual works permit. Inclusion of a covered work in an aggregate does not cause this License to apply to the other parts of the aggregate.
3.3. SOURCE CODE DISTRIBUTIONS. 6. Conveying Non-Source Forms.
When a Distributor makes the Licensed Work, or any portion thereof, available to any Person in Source Code form, it must be made available under this License and a copy of this License must be included with each copy of the Source Code, situated so that the copy of the License is conspicuously brought to the attention of that Person. For greater clarification, this Section 3.3 applies to all distribution of the Licensed Work in any Source Code form. A Distributor may charge a fee for the physical act of transferring a copy, which charge shall be no more than the cost of physically performing source distribution. You may convey a covered work in object code form under the terms of sections 4 and 5, provided that you also convey the machine-readable Corresponding Source under the terms of this License, in one of these ways:
3.4. REQUIRED NOTICES IN SOURCE CODE. a) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by the Corresponding Source fixed on a durable physical medium customarily used for software interchange.
Each Subsequent Contributor must ensure that the notice set out in Part 5 of Exhibit A is included in each file of the Source Code for each Subsequent Work originating from that particular Subsequent Contributor, if such notice is not already included in each such file. If it is not possible to put such notice in a particular Source Code file due to its structure, then the Subsequent Contributor must include such notice in a location (such as a relevant directory in which the file is stored) where a user would be likely to look for such a notice.
3.5. NO DISTRIBUTION REQUIREMENTS FOR INTERNALLY USED MODIFICATIONS. b) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by a written offer, valid for at least three years and valid for as long as you offer spare parts or customer support for that product model, to give anyone who possesses the object code either (1) a copy of the Corresponding Source for all the software in the product that is covered by this License, on a durable physical medium customarily used for software interchange, for a price no more than your reasonable cost of physically performing this conveying of source, or (2) access to copy the Corresponding Source from a network server at no charge.
Notwithstanding Sections 3.2, 3.3 and 3.4, Recipient may, internally within its own corporation or organization use the Licensed Work, including the Initial Work and Subsequent Works, and make Modifications for internal use within Recipient's own corporation or organization (collectively, "INTERNAL USE MODIFICATIONS"). The Recipient shall have no obligation to distribute, in either Source Code or Executable form, any such Internal Use Modifications made by Recipient in the course of such internal use, except where required below in this Section 3.5. All Internal Use Modifications distributed to any Person, whether or not a Third Party, shall be distributed pursuant to and be accompanied by the terms of this License. If the Recipient chooses to distribute any such Internal Use Modifications to any Third Party, then the Recipient shall be deemed a Subsequent Contributor, and any such Internal Use Modifications distributed to any Third Party shall be deemed a Subsequent Work originating from that Subsequent Contributor, and shall from the first such instance become part of the Licensed Work that must thereafter be distributed and made available to third parties in accordance with the terms of Sections 3.1 to 3.4 inclusive.
3.6. INDEPENDENT MODULES. c) Convey individual copies of the object code with a copy of the written offer to provide the Corresponding Source. This alternative is allowed only occasionally and noncommercially, and only if you received the object code with such an offer, in accord with subsection 6b.
This License shall not apply to Independent Modules of any Initial Contributor, Subsequent Contributor, Distributor or any Recipient, and such Independent Modules may be licensed or made available under one or more separate license agreements.
3.7. LARGER WORKS. d) Convey the object code by offering access from a designated place (gratis or for a charge), and offer equivalent access to the Corresponding Source in the same way through the same place at no further charge. You need not require recipients to copy the Corresponding Source along with the object code. If the place to copy the object code is a network server, the Corresponding Source may be on a different server (operated by you or a third party) that supports equivalent copying facilities, provided you maintain clear directions next to the object code saying where to find the Corresponding Source. Regardless of what server hosts the Corresponding Source, you remain obligated to ensure that it is available for as long as needed to satisfy these requirements.
Any Distributor or Recipient may create or contribute to a Larger Work by combining any of the Licensed Work with other code not governed by the terms of this License, and may distribute the Larger Work as one or more products. However, in any such case, Distributor or Recipient (as the case may be) must make sure that the requirements of this License are fulfilled for the Licensed Work portion of the Larger Work.
3.8. DESCRIPTION OF DISTRIBUTED MODIFICATIONS. e) Convey the object code using peer-to-peer transmission, provided you inform other peers where the object code and Corresponding Source of the work are being offered to the general public at no charge under subsection 6d.
(a) Each Subsequent Contributor (including the Initial Contributor where the Initial Contributor also qualifies as a Subsequent Contributor) must cause each Subsequent Work created or contributed to by that Subsequent Contributor to contain a file documenting the changes, in accordance with the requirements of Part 1 of the Supplement File, that such Subsequent Contributor made in the creation or contribution to that Subsequent Work. If no Supplement File exists or no requirements are set out in Part 1 of the Supplement File, then there are no requirements for Subsequent Contributors to document changes that they make resulting in Subsequent Works. A separable portion of the object code, whose source code is excluded from the Corresponding Source as a System Library, need not be included in conveying the object code work.
(b) The Initial Contributor may at any time introduce requirements or add to or change earlier requirements (in each case, the "EARLIER DESCRIPTION REQUIREMENTS") for documenting changes resulting in Subsequent Works by revising Part 1 of each copy of the Supplement File distributed by the Initial Contributor with future copies of the Licensed Work so that Part 1 then contains new requirements (the "NEW DESCRIPTION REQUIREMENTS") for documenting such changes.
(c) Any Recipient receiving at any time any copy of an Initial Work or any Subsequent Work under a copy of this License (in each case, an "Earlier LICENSED COPY") having the Earlier Description Requirements may choose, with respect to each such Earlier Licensed Copy, to comply with the Earlier Description Requirements or the New Description Requirements. Where a Recipient chooses to comply with the New Description Requirements, that Recipient will, when thereafter distributing any copies of any such Earlier Licensed Copy, include a Supplement File having a section entitled Part 1 that contains a copy of the New Description Requirements.
(d) For greater certainty, the intent of Part 1 of the Supplement File is to provide a mechanism (if any) by which Subsequent Contributors must document changes that they make to the Licensed Work resulting in Subsequent Works. Part 1 of any Supplement File shall not be used to increase or reduce the scope of the license granted in Article 2 of this License or in any other way increase or decrease the rights and obligations of any Recipient, and shall at no time serve as the basis for terminating the License. Further, a Recipient can be required to correct and change its documentation procedures to comply with Part 1 of the Supplement File, but cannot be penalised with damages. Part 1 of any Supplement File is only binding on each Recipient of any Licensed Work to the extent Part 1 sets out the requirements for documenting changes to the Initial Work or any Subsequent Work.
(e) An example of a set of requirements for documenting changes and contributions made by Subsequent Contributor is set out in Part 7 of Exhibit A of this License. Part 7 is a sample only and is not binding on Recipients, unless (subject to the earlier paragraphs of this Section 3.8) those are the requirements that the Initial Contributor includes in Part 1 of the Supplement File with the copies of the Initial Work distributed under this License.
3.9. USE OF DISTRIBUTOR NAME. A “User Product” is either (1) a “consumer product”, which means any tangible personal property which is normally used for personal, family, or household purposes, or (2) anything designed or sold for incorporation into a dwelling. In determining whether a product is a consumer product, doubtful cases shall be resolved in favor of coverage. For a particular product received by a particular user, “normally used” refers to a typical or common use of that class of product, regardless of the status of the particular user or of the way in which the particular user actually uses, or expects or is expected to use, the product. A product is a consumer product regardless of whether the product has substantial commercial, industrial or non-consumer uses, unless such uses represent the only significant mode of use of the product.
The name of a Distributor may not be used by any other Distributor to endorse or promote the Licensed Work or products derived from the Licensed Work, without prior written permission.
3.10. LIMITED RECOGNITION OF INITIAL CONTRIBUTOR. “Installation Information” for a User Product means any methods, procedures, authorization keys, or other information required to install and execute modified versions of a covered work in that User Product from a modified version of its Corresponding Source. The information must suffice to ensure that the continued functioning of the modified object code is in no case prevented or interfered with solely because modification has been made.
(a) As a modest attribution to the Initial Contributor, in the hope that its promotional value may help justify the time, money and effort invested in writing the Initial Work, the Initial Contributor may include in Part 2 of the Supplement File a requirement that each time an executable program resulting from the Initial Work or any Subsequent Work, or a program dependent thereon, is launched or run, a prominent display of the Initial Contributor's attribution information must occur (the "ATTRIBUTION INFORMATION"). The Attribution Information must be included at the beginning of each Source Code file. For greater certainty, the Initial Contributor may specify in the Supplement File that the above attribution requirement only applies to an executable program resulting from the Initial Work or any Subsequent Work, but not a program dependent thereon. The intent is to provide for reasonably modest attribution, therefore the Initial Contributor may not require Recipients to display, at any time, more than the following Attribution Information: (a) a copyright notice including the name of the Initial Contributor; (b) a word or one phrase (not exceeding 10 words); (c) one digital image or graphic provided with the Initial Work; and (d) a URL (collectively, the "ATTRIBUTION LIMITS"). If you convey an object code work under this section in, or with, or specifically for use in, a User Product, and the conveying occurs as part of a transaction in which the right of possession and use of the User Product is transferred to the recipient in perpetuity or for a fixed term (regardless of how the transaction is characterized), the Corresponding Source conveyed under this section must be accompanied by the Installation Information. But this requirement does not apply if neither you nor any third party retains the ability to install modified object code on the User Product (for example, the work has been installed in ROM).
(b) If no Supplement File exists, or no Attribution Information is set out in Part 2 of the Supplement File, then there are no requirements for Recipients to display any Attribution Information of the Initial Contributor.
(c) Each Recipient acknowledges that all trademarks, service marks and/or trade names contained within Part 2 of the Supplement File distributed with the Licensed Work are the exclusive property of the Initial Contributor and may only be used with the permission of the Initial Contributor, or under circumstances otherwise permitted by law, or as expressly set out in this License.
3.11. For greater certainty, any description or attribution provisions contained within a Supplement File may only be used to specify the nature of the description or attribution requirements, as the case may be. Any provision in a Supplement File that otherwise purports to modify, vary, nullify or amend any right, obligation or representation contained herein shall be deemed void to that extent, and shall be of no force or effect.
4. COMMERCIAL USE AND INDEMNITY. The requirement to provide Installation Information does not include a requirement to continue to provide support service, warranty, or updates for a work that has been modified or installed by the recipient, or for the User Product in which it has been modified or installed. Access to a network may be denied when the modification itself materially and adversely affects the operation of the network or violates the rules and protocols for communication across the network.
4.1. COMMERCIAL SERVICES. Corresponding Source conveyed, and Installation Information provided, in accord with this section must be in a format that is publicly documented (and with an implementation available to the public in source code form), and must require no special password or key for unpacking, reading or copying.
A Recipient ("COMMERCIAL RECIPIENT") may choose to offer, and to charge a fee for, warranty, support, indemnity or liability obligations (collectively, "SERVICES") to one or more other Recipients or Distributors. However, such Commercial Recipient may do so only on that Commercial Recipient's own behalf, and not on behalf of any other Distributor or Recipient, and Commercial Recipient must make it clear than any such warranty, support, indemnity or liability obligation(s) is/are offered by Commercial Recipient alone. At no time may Commercial Recipient use any Services to deny any party the Licensed Work in Source Code or Executable form when so required under any of the other terms of this License. For greater certainty, this Section 4.1 does not diminish any of the other terms of this License, including without limitation the obligation of the Commercial Recipient as a Distributor, when distributing any of the Licensed Work in Source Code or Executable form, to make such distribution royalty-free (subject to the right to charge a fee of no more than the cost of physically performing Source Code or Executable distribution (as the case may be)).
4.2. INDEMNITY. 7. Additional Terms.
Commercial distributors of software may accept certain responsibilities with respect to end users, business partners and the like. While this License is intended to facilitate the commercial use of the Licensed Work, the Distributor who includes any of the Licensed Work in a commercial product offering should do so in a manner which does not create potential liability for other Distributors. Therefore, if a Distributor includes the Licensed Work in a commercial product offering or offers any Services, such Distributor ("COMMERCIAL DISTRIBUTOR") hereby agrees to defend and indemnify every other Distributor or Subsequent Contributor (in each case an "INDEMNIFIED PARTY") against any losses, damages and costs (collectively "LOSSES") arising from claims, lawsuits and other legal actions brought by a third party against the Indemnified Party to the extent caused by the acts or omissions of such Commercial Distributor in connection with its distribution of any of the Licensed Work in a commercial product offering or in connection with any Services. The obligations in this section do not apply to any claims or Losses relating to any actual or alleged intellectual property infringement. In order to qualify, an Indemnified Party must: (a) promptly notify the Commercial Distributor in writing of such claim; and (b) allow the Commercial Distributor to control, and co-operate with the Commercial Distributor in, the defense and any related settlement negotiations. The Indemnified Party may participate in any such claim at its own expense. “Additional permissions” are terms that supplement the terms of this License by making exceptions from one or more of its conditions. Additional permissions that are applicable to the entire Program shall be treated as though they were included in this License, to the extent that they are valid under applicable law. If additional permissions apply only to part of the Program, that part may be used separately under those permissions, but the entire Program remains governed by this License without regard to the additional permissions.
5. VERSIONS OF THE LICENSE. When you convey a copy of a covered work, you may at your option remove any additional permissions from that copy, or from any part of it. (Additional permissions may be written to require their own removal in certain cases when you modify the work.) You may place additional permissions on material, added by you to a covered work, for which you have or can give appropriate copyright permission.
5.1. NEW VERSIONS. Notwithstanding any other provision of this License, for material you add to a covered work, you may (if authorized by the copyright holders of that material) supplement the terms of this License with terms:
The Initial Contributor may publish revised and/or new versions of the License from time to time. Each version will be given a distinguishing version number.
5.2. EFFECT OF NEW VERSIONS. a) Disclaiming warranty or limiting liability differently from the terms of sections 15 and 16 of this License; or
Once the Licensed Work or any portion thereof has been published by Initial Contributor under a particular version of the License, Recipient may choose to continue to use it under the terms of that version. However, if a Recipient chooses to use the Licensed Work under the terms of any subsequent version of the License published by the Initial Contributor, then from the date of making this choice, the Recipient must comply with the terms of that subsequent version with respect to all further reproduction, preparation of derivative works, public display of, public performance of, distribution and sublicensing by the Recipient in connection with the Licensed Work. No one other than the Initial Contributor has the right to modify the terms applicable to the Licensed Work
6. DISCLAIMER OF WARRANTY. b) Requiring preservation of specified reasonable legal notices or author attributions in that material or in the Appropriate Legal Notices displayed by works containing it; or
6.1. GENERAL DISCLAIMER. c) Prohibiting misrepresentation of the origin of that material, or requiring that modified versions of such material be marked in reasonable ways as different from the original version; or
EXCEPT AS EXPRESSLY SET FORTH IN THIS LICENSE, THE LICENSED WORK IS PROVIDED UNDER THIS LICENSE ON AN "AS IS" BASIS, WITHOUT ANY REPRESENTATION, WARRANTY, GUARANTEE, ASSURANCE OR CONDITION OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, WITHOUT LIMITATION, WARRANTIES OR CONDITIONS OF TITLE, NON-INFRINGEMENT, MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE LICENSED WORK IS WITH RECIPIENT. SHOULD ANY LICENSED WORK PROVE DEFECTIVE IN ANY RESPECT, RECIPIENT (NOT THE INITIAL CONTRIBUTOR OR ANY SUBSEQUENT CONTRIBUTOR) ASSUMES THE COST OF ANY NECESSARY SERVICING, REPAIR OR CORRECTION. THIS CLAUSE CONSTITUTES AN ESSENTIAL PART OF THIS LICENSE. NO USE OF ANY LICENSED WORK IS AUTHORIZED HEREUNDER EXCEPT UNDER THIS LICENSE INCLUDING WITHOUT LIMITATION THIS DISCLAIMER.
6.2. RESPONSIBILITY OF RECIPIENTS. d) Limiting the use for publicity purposes of names of licensors or authors of the material; or
Each Recipient is solely responsible for determining the appropriateness of using and distributing the Licensed Work and assumes all risks associated with its exercise of rights under this License, including but not limited to the risks and costs of program errors, compliance with applicable laws, damage to or loss of data, programs or equipment, and unavailability or interruption of operations.
7. TERMINATION. e) Declining to grant rights under trademark law for use of some trade names, trademarks, or service marks; or
7.1. This License shall continue until terminated in accordance with the express terms herein. f) Requiring indemnification of licensors and authors of that material by anyone who conveys the material (or modified versions of it) with contractual assumptions of liability to the recipient, for any liability that these contractual assumptions directly impose on those licensors and authors.
7.2. Recipient may choose to terminate this License automatically at any time. All other non-permissive additional terms are considered “further restrictions” within the meaning of section 10. If the Program as you received it, or any part of it, contains a notice stating that it is governed by this License along with a term that is a further restriction, you may remove that term. If a license document contains a further restriction but permits relicensing or conveying under this License, you may add to a covered work material governed by the terms of that license document, provided that the further restriction does not survive such relicensing or conveying.
7.3. This License, including without limitation the rights granted hereunder to a particular Recipient, will terminate automatically if such Recipient is in material breach of any of the terms of this License and fails to cure such breach within sixty (60) days of becoming aware of the breach. Without limiting the foregoing, any material breach by such Recipient of any term of any other License under which such Recipient is granted any rights to the Licensed Work shall constitute a material breach of this License. If you add terms to a covered work in accord with this section, you must place, in the relevant source files, a statement of the additional terms that apply to those files, or a notice indicating where to find the applicable terms.
7.4. Upon termination of this License by or with respect to a particular Recipient for any reason, all rights granted hereunder and under any other License to that Recipient shall terminate. However, all sublicenses to the Licensed Work which were previously properly granted by such Recipient under a copy of this License (in each case, an "Other License" and in plural, "Other Licenses") shall survive any such termination of this License, including without limitation the rights and obligations under such Other Licenses as set out in their respective Sections 2, 3, 4, 5, 6, 7 and 8, mutatis mutandis, for so long as the respective sublicensees (i.e. other Recipients) remain in compliance with the terms of the copy of this License under which such sublicensees received rights to the Licensed Work. Any termination of such Other Licenses shall be pursuant to their respective Section 7, mutatis mutandis. Provisions which, by their nature, must remain in effect beyond the termination of this License shall survive. Additional terms, permissive or non-permissive, may be stated in the form of a separately written license, or stated as exceptions; the above requirements apply either way.
7.5. Upon any termination of this License by or with respect to a particular Recipient, Sections 4.1, 4.2, 6.1, 6.2, 7.4, 7.5, 8.1, and 8.2, together with all provisions of this License necessary for the interpretation and enforcement of same, shall expressly survive such termination. 8. Termination.
You may not propagate or modify a covered work except as expressly provided under this License. Any attempt otherwise to propagate or modify it is void, and will automatically terminate your rights under this License (including any patent licenses granted under the third paragraph of section 11).
8. LIMITATION OF LIABILITY. However, if you cease all violation of this License, then your license from a particular copyright holder is reinstated (a) provisionally, unless and until the copyright holder explicitly and finally terminates your license, and (b) permanently, if the copyright holder fails to notify you of the violation by some reasonable means prior to 60 days after the cessation.
8.1. IN NO EVENT SHALL ANY OF INITIAL CONTRIBUTOR, ITS SUBSIDIARIES, OR AFFILIATES, OR ANY OF ITS OR THEIR RESPECTIVE OFFICERS, DIRECTORS, EMPLOYEES, AND/OR AGENTS (AS THE CASE MAY BE), HAVE ANY LIABILITY FOR ANY DIRECT DAMAGES, INDIRECT DAMAGES, PUNITIVE DAMAGES, INCIDENTAL DAMAGES, SPECIAL DAMAGES, EXEMPLARY DAMAGES, CONSEQUENTIAL DAMAGES OR ANY OTHER DAMAGES WHATSOEVER (INCLUDING WITHOUT LIMITATION LOSS OF USE, DATA OR PROFITS, OR ANY OTHER LOSS ARISING OUT OF OR IN ANY WAY RELATED TO THE USE, INABILITY TO USE, UNAUTHORIZED USE, PERFORMANCE, OR NON-PERFORMANCE OF THE LICENSED WORK OR ANY PART THEREOF OR THE PROVISION OF OR FAILURE TO PROVIDE SUPPORT SERVICES, OR THAT RESULT FROM ERRORS, DEFECTS, OMISSIONS, DELAYS IN OPERATION OR TRANSMISSION, OR ANY OTHER FAILURE OF PERFORMANCE), HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) IN RELATION TO OR ARISING IN ANY WAY OUT OF THIS LICENSE OR THE USE OR DISTRIBUTION OF THE LICENSED WORK OR THE EXERCISE OF ANY RIGHTS GRANTED HEREUNDER, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. THIS LIMITATION OF LIABILITY SHALL NOT APPLY TO LIABILITY FOR DEATH OR PERSONAL INJURY RESULTING FROM SUCH PARTY'S NEGLIGENCE TO THE EXTENT APPLICABLE LAW PROHIBITS SUCH LIMITATION. THIS CLAUSE CONSTITUTES AN ESSENTIAL PART OF THIS LICENSE. NO USE OF ANY LICENSED WORK IS AUTHORIZED HEREUNDER EXCEPT UNDER THIS LICENSE INCLUDING WITHOUT LIMITATION THE LIMITATIONS SET FORTH IN THIS SECTION 8.1. Moreover, your license from a particular copyright holder is reinstated permanently if the copyright holder notifies you of the violation by some reasonable means, this is the first time you have received notice of violation of this License (for any work) from that copyright holder, and you cure the violation prior to 30 days after your receipt of the notice.
8.2. EXCEPT AS EXPRESSLY SET FORTH IN THIS LICENSE, EACH RECIPIENT SHALL NOT HAVE ANY LIABILITY FOR ANY EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING WITHOUT LIMITATION LOST PROFITS), HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OR DISTRIBUTION OF THE LICENSED WORK OR THE EXERCISE OF ANY RIGHTS GRANTED HEREUNDER, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. THIS LIMITATION OF LIABILITY SHALL NOT APPLY TO LIABILITY FOR DEATH OR PERSONAL INJURY RESULTING FROM SUCH PARTY'S NEGLIGENCE TO THE EXTENT APPLICABLE LAW PROHIBITS SUCH LIMITATION. Termination of your rights under this section does not terminate the licenses of parties who have received copies or rights from you under this License. If your rights have been terminated and not permanently reinstated, you do not qualify to receive new licenses for the same material under section 10.
9. GOVERNING LAW AND LEGAL ACTION. 9. Acceptance Not Required for Having Copies.
You are not required to accept this License in order to receive or run a copy of the Program. Ancillary propagation of a covered work occurring solely as a consequence of using peer-to-peer transmission to receive a copy likewise does not require acceptance. However, nothing other than this License grants you permission to propagate or modify any covered work. These actions infringe copyright if you do not accept this License. Therefore, by modifying or propagating a covered work, you indicate your acceptance of this License to do so.
9.1. This License shall be governed by and construed in accordance with the laws of the Governing Jurisdiction assigned in Part 3 of Exhibit A, without regard to its conflict of law provisions. No party may bring a legal action under this License more than one year after the cause of the action arose. Each party waives its rights (if any) to a jury trial in any litigation arising under this License. Note that if the Governing Jurisdiction is not assigned in Part 3 of Exhibit A, then the Governing Jurisdiction shall be the State of New York. 10. Automatic Licensing of Downstream Recipients.
Each time you convey a covered work, the recipient automatically receives a license from the original licensors, to run, modify and propagate that work, subject to this License. You are not responsible for enforcing compliance by third parties with this License.
9.2. The courts of the Governing Jurisdiction shall have jurisdiction, but not exclusive jurisdiction, to entertain and determine all disputes and claims, whether for specific performance, injunction, damages or otherwise, both at law and in equity, arising out of or in any way relating to this License, including without limitation, the legality, validity, existence and enforceability of this License. Each party to this License hereby irrevocably attorns to and accepts the jurisdiction of the courts of the Governing Jurisdiction for such purposes. An “entity transaction” is a transaction transferring control of an organization, or substantially all assets of one, or subdividing an organization, or merging organizations. If propagation of a covered work results from an entity transaction, each party to that transaction who receives a copy of the work also receives whatever licenses to the work the party's predecessor in interest had or could give under the previous paragraph, plus a right to possession of the Corresponding Source of the work from the predecessor in interest, if the predecessor has it or can get it with reasonable efforts.
9.3. Except as expressly set forth elsewhere herein, in the event of any action or proceeding brought by any party against another under this License the prevailing party shall be entitled to recover all costs and expenses including the fees of its attorneys in such action or proceeding in such amount as the court may adjudge reasonable. You may not impose any further restrictions on the exercise of the rights granted or affirmed under this License. For example, you may not impose a license fee, royalty, or other charge for exercise of rights granted under this License, and you may not initiate litigation (including a cross-claim or counterclaim in a lawsuit) alleging that any patent claim is infringed by making, using, selling, offering for sale, or importing the Program or any portion of it.
10. MISCELLANEOUS. 11. Patents.
A “contributor” is a copyright holder who authorizes use under this License of the Program or a work on which the Program is based. The work thus licensed is called the contributor's “contributor version”.
10.1. The obligations imposed by this License are for the benefit of the Initial Contributor and any Recipient, and each Recipient acknowledges and agrees that the Initial Contributor and/or any other Recipient may enforce the terms and conditions of this License against any Recipient. A contributor's “essential patent claims” are all patent claims owned or controlled by the contributor, whether already acquired or hereafter acquired, that would be infringed by some manner, permitted by this License, of making, using, or selling its contributor version, but do not include claims that would be infringed only as a consequence of further modification of the contributor version. For purposes of this definition, “control” includes the right to grant patent sublicenses in a manner consistent with the requirements of this License.
10.2. This License represents the complete agreement concerning subject matter hereof, and supersedes and cancels all previous oral and written communications, representations, agreements and understandings between the parties with respect to the subject matter hereof. Each contributor grants you a non-exclusive, worldwide, royalty-free patent license under the contributor's essential patent claims, to make, use, sell, offer for sale, import and otherwise run, modify and propagate the contents of its contributor version.
10.3. The application of the United Nations Convention on Contracts for the International Sale of Goods is expressly excluded. In the following three paragraphs, a “patent license” is any express agreement or commitment, however denominated, not to enforce a patent (such as an express permission to practice a patent or covenant not to sue for patent infringement). To “grant” such a patent license to a party means to make such an agreement or commitment not to enforce a patent against the party.
10.4. The language in all parts of this License shall be in all cases construed simply according to its fair meaning, and not strictly for or against any of the parties hereto. Any law or regulation which provides that the language of a contract shall be construed against the drafter shall not apply to this License. If you convey a covered work, knowingly relying on a patent license, and the Corresponding Source of the work is not available for anyone to copy, free of charge and under the terms of this License, through a publicly available network server or other readily accessible means, then you must either (1) cause the Corresponding Source to be so available, or (2) arrange to deprive yourself of the benefit of the patent license for this particular work, or (3) arrange, in a manner consistent with the requirements of this License, to extend the patent license to downstream recipients. “Knowingly relying” means you have actual knowledge that, but for the patent license, your conveying the covered work in a country, or your recipient's use of the covered work in a country, would infringe one or more identifiable patents in that country that you have reason to believe are valid.
10.5. If any provision of this License is invalid or unenforceable under the laws of the Governing Jurisdiction, it shall not affect the validity or enforceability of the remainder of the terms of this License, and without further action by the parties hereto, such provision shall be reformed to the minimum extent necessary to make such provision valid and enforceable. If, pursuant to or in connection with a single transaction or arrangement, you convey, or propagate by procuring conveyance of, a covered work, and grant a patent license to some of the parties receiving the covered work authorizing them to use, propagate, modify or convey a specific copy of the covered work, then the patent license you grant is automatically extended to all recipients of the covered work and works based on it.
10.6. The paragraph headings of this License are for reference and convenience only and are not a part of this License, and they shall have no effect upon the construction or interpretation of any part hereof. A patent license is “discriminatory” if it does not include within the scope of its coverage, prohibits the exercise of, or is conditioned on the non-exercise of one or more of the rights that are specifically granted under this License. You may not convey a covered work if you are a party to an arrangement with a third party that is in the business of distributing software, under which you make payment to the third party based on the extent of your activity of conveying the work, and under which the third party grants, to any of the parties who would receive the covered work from you, a discriminatory patent license (a) in connection with copies of the covered work conveyed by you (or copies made from those copies), or (b) primarily for and in connection with specific products or compilations that contain the covered work, unless you entered into that arrangement, or that patent license was granted, prior to 28 March 2007.
10.7. Each of the terms "including", "include" and "includes", when used in this License, is not limiting whether or not non-limiting language (such as "without limitation" or "but not limited to" or words of similar import) is used with reference thereto. Nothing in this License shall be construed as excluding or limiting any implied license or other defenses to infringement that may otherwise be available to you under applicable patent law.
10.8. The parties hereto acknowledge they have expressly required that this License and notices relating thereto be drafted in the English language. 12. No Surrender of Others' Freedom.
If conditions are imposed on you (whether by court order, agreement or otherwise) that contradict the conditions of this License, they do not excuse you from the conditions of this License. If you cannot convey a covered work so as to satisfy simultaneously your obligations under this License and any other pertinent obligations, then as a consequence you may not convey it at all. For example, if you agree to terms that obligate you to collect a royalty for further conveying from those to whom you convey the Program, the only way you could satisfy both those terms and this License would be to refrain entirely from conveying the Program.
//***THE LICENSE TERMS END HERE (OTHER THAN AS SET OUT IN EXHIBIT A).***// 13. Use with the GNU Affero General Public License.
Notwithstanding any other provision of this License, you have permission to link or combine any covered work with a work licensed under version 3 of the GNU Affero General Public License into a single combined work, and to convey the resulting work. The terms of this License will continue to apply to the part which is the covered work, but the special requirements of the GNU Affero General Public License, section 13, concerning interaction through a network will apply to the combination as such.
EXHIBIT A (to the Adaptive Public License) 14. Revised Versions of this License.
The Free Software Foundation may publish revised and/or new versions of the GNU General Public License from time to time. Such new versions will be similar in spirit to the present version, but may differ in detail to address new problems or concerns.
PART 1: INITIAL CONTRIBUTOR AND DESIGNATED WEB SITE Each version is given a distinguishing version number. If the Program specifies that a certain numbered version of the GNU General Public License “or any later version” applies to it, you have the option of following the terms and conditions either of that numbered version or of any later version published by the Free Software Foundation. If the Program does not specify a version number of the GNU General Public License, you may choose any version ever published by the Free Software Foundation.
The Initial Contributor is: If the Program specifies that a proxy can decide which future versions of the GNU General Public License can be used, that proxy's public statement of acceptance of a version permanently authorizes you to choose that version for the Program.
____________________________________________________
 
[Enter full name of Initial Contributor]
Address of Initial Contributor: Later license versions may give you additional or different permissions. However, no additional obligations are imposed on any author or copyright holder as a result of your choosing to follow a later version.
________________________________________________
 
________________________________________________
 
________________________________________________
 
[Enter address above]
The Designated Web Site is: 15. Disclaimer of Warranty.
__________________________________________________ THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM “AS IS” WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
 
[Enter URL for Designated Web Site of Initial Contributor]
NOTE: The Initial Contributor is to complete this Part 1, along with Parts 2, 3, and 5, and, if applicable, Parts 4 and 6. 16. Limitation of Liability.
IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES.
PART 2: INITIAL WORK 17. Interpretation of Sections 15 and 16.
If the disclaimer of warranty and limitation of liability provided above cannot be given local legal effect according to their terms, reviewing courts shall apply local law that most closely approximates an absolute waiver of all civil liability in connection with the Program, unless a warranty or assumption of liability accompanies a copy of the Program in return for a fee.
The Initial Work comprises the computer program(s) distributed by the Initial Contributor having the following title(s): _______________________________________________. END OF TERMS AND CONDITIONS
The date on which the Initial Work was first available under this License: _________________ How to Apply These Terms to Your New Programs
PART 3: GOVERNING JURISDICTION If you develop a new program, and you want it to be of the greatest possible use to the public, the best way to achieve this is to make it free software which everyone can redistribute and change under these terms.
For the purposes of this License, the Governing Jurisdiction is _________________________________________________. 
[Initial Contributor to Enter Governing Jurisdiction here] To do so, attach the following notices to the program. It is safest to attach them to the start of each source file to most effectively state the exclusion of warranty; and each file should have at least the “copyright” line and a pointer to where the full notice is found.
PART 4: THIRD PARTIES Lab2_MachineVision_PickAndPlace
Copyright (C) 2025 MachineVision.EENG4
For the purposes of this License, "Third Party" has the definition set forth below in the ONE paragraph selected by the Initial Contributor from paragraphs A, B, C, D and E when the Initial Work is distributed or otherwise made available by the Initial Contributor. To select one of the following paragraphs, the Initial Contributor must place an "X" or "x" in the selection box alongside the one respective paragraph selected. This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
SELECTION
 
BOX PARAGRAPH
[  ] A. "THIRD PARTY" means any third party.
 
 
[  ] B. "THIRD PARTY" means any third party except for any of the following: (a) a wholly owned subsidiary of the Subsequent Contributor in question; (b) a legal entity (the "PARENT") that wholly owns the Subsequent Contributor in question; or (c) a wholly owned subsidiary of the wholly owned subsidiary in (a) or of the Parent in (b).
 
 
[  ] C. "THIRD PARTY" means any third party except for any of the following: (a) any Person directly or indirectly owning a majority of the voting interest in the Subsequent Contributor or (b) any Person in which the Subsequent Contributor directly or indirectly owns a majority voting interest.
 
 
[  ] D. "THIRD PARTY" means any third party except for any Person directly or indirectly controlled by the Subsequent Contributor. For purposes of this definition, "control" shall mean the power to direct or cause the direction of, the management and policies of such Person whether through the ownership of voting interests, by contract, or otherwise.
 
 
[  ] E. "THIRD PARTY" means any third party except for any Person directly or indirectly controlling, controlled by, or under common control with the Subsequent Contributor. For purposes of this definition, "control" shall mean the power to direct or cause the direction of, the management and policies of such Person whether through the ownership of voting interests, by contract, or otherwise.
The default definition of "THIRD PARTY" is the definition set forth in paragraph A, if NONE OR MORE THAN ONE of paragraphs A, B, C, D or E in this Part 4 are selected by the Initial Contributor.
PART 5: NOTICE This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
THE LICENSED WORK IS PROVIDED UNDER THE TERMS OF THE ADAPTIVE PUBLIC LICENSE ("LICENSE") AS FIRST COMPLETED BY: ______________________ [Insert the name of the Initial Contributor here]. ANY USE, PUBLIC DISPLAY, PUBLIC PERFORMANCE, REPRODUCTION OR DISTRIBUTION OF, OR PREPARATION OF DERIVATIVE WORKS BASED ON, THE LICENSED WORK CONSTITUTES RECIPIENT'S ACCEPTANCE OF THIS LICENSE AND ITS TERMS, WHETHER OR NOT SUCH RECIPIENT READS THE TERMS OF THE LICENSE. "LICENSED WORK" AND "RECIPIENT" ARE DEFINED IN THE LICENSE. A COPY OF THE LICENSE IS LOCATED IN THE TEXT FILE ENTITLED "LICENSE.TXT" ACCOMPANYING THE CONTENTS OF THIS FILE. IF A COPY OF THE LICENSE DOES NOT ACCOMPANY THIS FILE, A COPY OF THE LICENSE MAY ALSO BE OBTAINED AT THE FOLLOWING WEB SITE: ___________________________________________________[Insert Initial Contributor's Designated Web Site here] You should have received a copy of the GNU General Public License along with this program. If not, see <https://www.gnu.org/licenses/>.
Software distributed under the License is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See the License for the specific language governing rights and limitations under the License. Also add information on how to contact you by electronic and paper mail.
PART 6: PATENT LICENSING TERMS If the program does terminal interaction, make it output a short notice like this when it starts in an interactive mode:
For the purposes of this License, paragraphs A, B, C, D and E of this Part 6 of Exhibit A are only incorporated and form part of the terms of the License if the Initial Contributor places an "X" or "x" in the selection box alongside the YES answer to the question immediately below. Lab2_MachineVision_PickAndPlace Copyright (C) 2025 MachineVision.EENG4
This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
This is free software, and you are welcome to redistribute it under certain conditions; type `show c' for details.
Is this a Patents-Included License pursuant to Section 2.2 of the License? The hypothetical commands `show w' and `show c' should show the appropriate parts of the General Public License. Of course, your program's commands might be different; for a GUI interface, you would use an “about box”.
YES [      ]
NO [      ]
By default, if YES is not selected by the Initial Contributor, the answer is NO. You should also get your employer (if you work as a programmer) or school, if any, to sign a “copyright disclaimer” for the program, if necessary. For more information on this, and how to apply and follow the GNU GPL, see <https://www.gnu.org/licenses/>.
A. For the purposes of the paragraphs in this Part 6 of Exhibit A, "LICENSABLE" means having the right to grant, to the maximum extent possible, whether at the time of the initial grant or subsequently acquired, any and all of the rights granted herein. The GNU General Public License does not permit incorporating your program into proprietary programs. If your program is a subroutine library, you may consider it more useful to permit linking proprietary applications with the library. If this is what you want to do, use the GNU Lesser General Public License instead of this License. But first, please read <https://www.gnu.org/philosophy/why-not-lgpl.html>.
B. The Initial Contributor hereby grants all Recipients a world-wide, royalty-free, non-exclusive license, subject to third party intellectual property claims, under patent claim(s) Licensable by the Initial Contributor that are or would be infringed by the making, using, selling, offering for sale, having made, importing, exporting, transfer or disposal of such Initial Work or any portion thereof. Notwithstanding the foregoing, no patent license is granted under this Paragraph B by the Initial Contributor: (1) for any code that the Initial Contributor deletes from the Initial Work (or any portion thereof) distributed by the Initial Contributor prior to such distribution; (2) for any Modifications made to the Initial Work (or any portion thereof) by any other Person; or (3) separate from the Initial Work (or portions thereof) distributed or made available by the Initial Contributor.
C. Effective upon distribution by a Subsequent Contributor to a Third Party of any Modifications made by that Subsequent Contributor, such Subsequent Contributor hereby grants all Recipients a world-wide, royalty-free, non-exclusive license, subject to third party intellectual property claims, under patent claim(s) Licensable by such Subsequent Contributor that are or would be infringed by the making, using, selling, offering for sale, having made, importing, exporting, transfer or disposal of any such Modifications made by that Subsequent Contributor alone and/or in combination with its Subsequent Work (or portions of such combination) to make, use, sell, offer for sale, have made, import, export, transfer and otherwise dispose of:
(1) Modifications made by that Subsequent Contributor (or portions thereof); and
(2) the combination of Modifications made by that Subsequent Contributor with its Subsequent Work (or portions of such combination);
(collectively and in each case, the "SUBSEQUENT CONTRIBUTOR VERSION").
Notwithstanding the foregoing, no patent license is granted under this Paragraph C by such Subsequent Contributor: (1) for any code that such Subsequent Contributor deletes from the Subsequent Contributor Version (or any portion thereof) distributed by the Subsequent Contributor prior to such distribution; (2) for any Modifications made to the Subsequent Contributor Version (or any portion thereof) by any other Person; or (3) separate from the Subsequent Contributor Version (or portions thereof) distributed or made available by the Subsequent Contributor.
D. Effective upon distribution of any Licensed Work by a Distributor to a Third Party, such Distributor hereby grants all Recipients a world-wide, royalty-free, non-exclusive license, subject to third party intellectual property claims, under patent claim(s) Licensable by such Distributor that are or would be infringed by the making, using, selling, offering for sale, having made, importing, exporting, transfer or disposal of any such Licensed Work distributed by such Distributor, to make, use, sell, offer for sale, have made, import, export, transfer and otherwise dispose of such Licensed Work or portions thereof (collectively and in each case, the "DISTRIBUTOR VERSION"). Notwithstanding the foregoing, no patent license is granted under this Paragraph D by such Distributor: (1) for any code that such Distributor deletes from the Distributor Version (or any portion thereof) distributed by the Distributor prior to such distribution; (2) for any Modifications made to the Distributor Version (or any portion thereof) by any other Person; or (3) separate from the Distributor Version (or portions thereof) distributed or made available by the Distributor.
E. If Recipient institutes patent litigation against another Recipient (a "USER") with respect to a patent applicable to a computer program or software (including a cross-claim or counterclaim in a lawsuit, and whether or not any of the patent claims are directed to a system, method, process, apparatus, device, product, article of manufacture or any other form of patent claim), then any patent or copyright license granted by that User to such Recipient under this License or any other copy of this License shall terminate. The termination shall be effective ninety (90) days after notice of termination from User to Recipient, unless the Recipient withdraws the patent litigation claim before the end of the ninety (90) day period. To be effective, any such notice of license termination must include a specific list of applicable patents and/or a copy of the copyrighted work of User that User alleges will be infringed by Recipient upon License termination. License termination is only effective with respect to patents and/or copyrights for which proper notice has been given.
PART 7: SAMPLE REQUIREMENTS FOR THE DESCRIPTION OF DISTRIBUTED MODIFICATIONS
Each Subsequent Contributor (including the Initial Contributor where the Initial Contributor qualifies as a Subsequent Contributor) is invited (but not required) to cause each Subsequent Work created or contributed to by that Subsequent Contributor to contain a file documenting the changes such Subsequent Contributor made to create that Subsequent Work and the date of any change. //***EXHIBIT A ENDS HERE.***//

BIN
bin/calibrateCamRobotReferenceFrames Normal file
View File

Binary file not shown.

BIN
bin/cartControl Normal file
View File

Binary file not shown.

BIN
bin/jointControl Normal file
View File

Binary file not shown.

BIN
bin/pickAndPlace Normal file
View File

Binary file not shown.

BIN
bin/trackCube Normal file
View File

Binary file not shown.

13
data/camrobot_params.xml Normal file
View File

@ -0,0 +1,13 @@
<?xml version="1.0"?>
<opencv_storage>
<robotCamTransform type_id="opencv-matrix">
<rows>4</rows>
<cols>4</cols>
<dt>d</dt>
<data>
2.5029447730113963e-01 7.8303949506316339e-01 5.6938723537130276e-01
-3.6475626774437458e+01 9.6481772705633306e-01
-2.5062907863178047e-01 -7.9446954013579804e-02
9.1202978564642425e+00 8.0494895430706426e-02 5.6924003207382645e-01
-8.1822146005478813e-01 5.6500640505571752e+01 0. 0. 0. 1.</data></robotCamTransform>
</opencv_storage>

14
data/cma_robot_params.xml Normal file
View File

@ -0,0 +1,14 @@
<?xml version="1.0"?>
<opencv_storage>
<robotCamTransform type_id="opencv-matrix">
<rows>4</rows>
<cols>4</cols>
<dt>d</dt>
<data>
3.8701658991758081e-01 -8.8988565609780756e-01
2.4149881614604551e-01 -1.9097667500272575e+01
-9.2002075245445802e-01 -3.9013907529317599e-01
3.6787456863964139e-02 -1.0054842804846023e+01
6.1481494628056077e-02 -2.3642127865477694e-01
-9.6970356543509761e-01 7.1571122924229471e+01 0. 0. 0. 1.</data></robotCamTransform>
</opencv_storage>

9
data/color_params.xml Normal file
View File

@ -0,0 +1,9 @@
<?xml version="1.0"?>
<opencv_storage>
<lowH>35</lowH>
<highH>75</highH>
<lowS>101</lowS>
<highS>255</highS>
<lowV>20</lowV>
<highV>255</highV>
</opencv_storage>

165079
data/stereo_params.xml Normal file
View File

File diff suppressed because it is too large Load Diff

95
include/DynamixelHandler.h Normal file
View File

@ -0,0 +1,95 @@
#ifndef _DYNAMIXEL_HANDLER_
#define _DYNAMIXEL_HANDLER_
#if defined(__linux__) || defined(__APPLE__)
#include <fcntl.h>
#include <getopt.h>
#include <termios.h>
#define STDIN_FILENO 0
#elif defined(_WIN32) || defined(_WIN64)
#include <conio.h>
#endif
#define _USE_MATH_DEFINES
#include <cmath>
// standard includes
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <iostream>
#include <vector>
// dynamixel sdk include
#include "dynamixel_sdk/dynamixel_sdk.h"
// addresses of variables in the register
#define ADDR_XL320_CONTROL_MODE 11
#define ADDR_XL320_TORQUE_ENABLE 24
#define ADDR_XL320_GOAL_POSITION 30
#define ADDR_XL320_GOAL_VELOCITY 32
#define ADDR_XL320_PRESENT_POSITION 37
#define ADDR_XL320_PRESENT_VELOCITY 39
#define ADDR_XL320_HARDWARE_ERROR_STATUS 50
// rotation direction
#define ROT_DIRECTION_Q1 1
#define ROT_DIRECTION_Q2 -1
#define ROT_DIRECTION_Q3 -1
#define ROT_DIRECTION_QGRIPPER 1
// nb of joints
#define NB_JOINTS 4
class DynamixelHandler
{
public:
DynamixelHandler();
~DynamixelHandler();
public:
bool openPort();
void closePort();
bool setBaudRate(int);
void setDeviceName(std::string);
void setProtocolVersion(float);
bool enableTorque(bool);
bool setControlMode(int iControlMode);
bool readCurrentJointPosition(std::vector<uint16_t>& vCurrentJointPosition);
bool readCurrentJointPosition(std::vector<float>& vCurrentJointPosition);
bool sendTargetJointPosition(std::vector<uint16_t>& vTargetJointPosition);
bool sendTargetJointPosition(std::vector<float>& vTargetJointPosition);
bool controlGripper(float f32GripperAngle);
bool sendTargetJointVelocity(std::vector<uint16_t>& vTargetJointVelocity);
bool sendTargetJointVelocity(std::vector<float>& vTargetJointVelocity);
int convertAngleToJointCmd(float fJointAngle);
float convertJointCmdToAngle(int iJointCmd);
int convertJointVelocityToJointCmd(float fJointVelocity);
private:
std::string m_sDeviceName;
float m_fProtocolVersion;
int m_i32BaudRate;
dynamixel::PortHandler* m_pPortHandler;
dynamixel::PacketHandler* m_pPacketHandler;
bool m_bIsDeviceNameSet;
bool m_bIsProtocolVersionSet;
bool m_bIsPortOpened;
bool m_bIsBaudRateSet;
int m_i32DxlCommunicationResult; // Communication result
uint8_t m_ui8DxlError; // Dynamixel error
std::vector<uint16_t> m_vDxlCurrentPosition; // Present position
float m_fMinJointCmd = 0;
float m_fMaxJointCmd = 1023;
float m_fMinJointAngle = -150.0f/180.0f*M_PI;
float m_fMaxJointAngle = 150.0f/180.0f*M_PI;
};
#endif

40
include/ImageProcessing.h Normal file
View File

@ -0,0 +1,40 @@
#ifndef _IMAGE_PROCESSING_
#define _IMAGE_PROCESSING_
#include <iostream>
#include <vector>
#include "opencv2/opencv.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"
//~ #include "DynamixelHandler.h"
//~ #include "Kinematics.h"
// ImageProcessing
#define thresholdValue 200
#define maxValue 255
//ConvertContours
#define drawingAreaWidthInMm 80.0
#define drawingAreaHeigthInMm 50.0
// SendContours
#define deltaTBetweenSamples 500 //in ms
#define incrementBetweenSamples 2
#define DISPARITY_WIN_SIZE 800
std::vector<std::vector<cv::Point>> imageProcessing(cv::Mat originalImage);
std::vector<std::vector<float>> convertContoursPixel2Mm(std::vector<std::vector<cv::Point>> vContoursInPixel, float theta, float tx, float ty, int imageWidth, int imageHeight);
//~ void sendContours(std::vector<std::vector<cv::Point>> vContoursInPixel, std::vector<std::vector<float>> vContoursInMm, DynamixelHandler& dxlHandler, float L1, float L2, float L3);
void findObject2DPosition(cv::Mat frame, cv::Mat& frameThresholded, bool bIsImageUndistorted, bool isCamParamsSet, cv::Mat cameraMatrix, cv::Mat distCoeffs, int iLowH, int iLowS, int iLowV, int iHighH, int iHighS, int iHighV, int iStructuralElementSize, int iAreaThresold, int& iLastX, int& iLastY, cv::Mat& imgLines);
void findObject2DPosition(cv::Mat frame, cv::Mat& frameThresholded, int iLowH, int iLowS, int iLowV, int iHighH, int iHighS, int iHighV, int iStructuralElementSize, int iAreaThresold, int& iLastX, int& iLastY, cv::Mat& imgLines);
void findObject3DPosition(int iLastXR, int iLastYR, int iLastXL, int iLastYL, cv::Mat frameGray, cv::Mat Q, double& x, double& y, double& z);
#endif

25
include/Kinematics.h Normal file
View File

@ -0,0 +1,25 @@
#ifndef _KINEMATICS_
#define _KINEMATICS_
#define _USE_MATH_DEFINES
#include <cmath>
#include <vector>
#include <iostream>
#include "opencv2/opencv.hpp"
float deg2rad(float angle);
float rad2deg(float angle);
std::vector<float> computeForwardKinematics(float q1, float q2, float q3, float L1, float L2, float L3);
std::vector<float> computeInverseKinematics(float x, float y, float z, float L1, float L2, float L3);
std::vector<float> computeDifferentialKinematics(float q1, float q2, float q3, float L1, float L2, float L3);
int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold);
cv::Mat computeInverseJacobianMatrix(std::vector<float> vJacobianMatrix);
#endif

25
include/ReadWriteFunctions.h Normal file
View File

@ -0,0 +1,25 @@
#ifndef _READWRITE_FUNCTIONS_
#define _READWRITE_FUNCTIONS_
#include <opencv2/opencv.hpp>
bool readCamRobotCalibrationParameters(std::string filename, cv::Mat& robotCamTransform);
bool readMonoCameraCalibParameters(std::string filename, cv::Mat &camMatrix, cv::Mat & distCoeffs);
bool readStereoCameraParameters(std::string filename, cv::Mat &camLMatrix, cv::Mat & distLCoeffs, cv::Mat &camRMatrix, cv::Mat & distRCoeffs, cv::Mat &stereoMapL_x, cv::Mat &stereoMapL_y, cv::Mat &stereoMapR_x, cv::Mat &stereoMapR_y, cv::Mat &Q);
bool readDisparityParameters(std::string filename, int &numDisparities, int &blockSize , int &preFilterType, int &preFilterSize, int &preFilterCap, int &minDisparity , int &textureThreshold, int &uniquenessRatio, int &speckleRange, int &speckleWindowSize, int &disp12MaxDiff);
bool readColorParameters(std::string filename, int& iLowH, int& iHighH, int& iLowS, int& iHighS, int& iLowV, int& iHighV);
bool writeMonoCameraParameters(std::string filename, cv::Mat camMatrix, cv::Mat distCoeffs);
bool writeColorParameters(std::string filename, int iLowH, int iHighH, int iLowS, int iHighS, int iLowV, int iHighV);
bool writeStereoCameraParameters(std::string filename, cv::Mat camLMatrix, cv::Mat distLCoeffs, cv::Mat camRMatrix, cv::Mat distRCoeffs, cv::Mat stereoMapL_x, cv::Mat stereoMapL_y, cv::Mat stereoMapR_x, cv::Mat stereoMapR_y, cv::Mat Q);
bool writeDisparityParameters(std::string filename, int numDisparities, int blockSize , int preFilterType, int preFilterSize, int preFilterCap, int minDisparity , int textureThreshold, int uniquenessRatio, int speckleRange, int speckleWindowSize, int disp12MaxDiff);
bool writeCamRobotCalibrationParameters(std::string filename, cv::Mat robotCamTransform);
#endif

14
include/RobotControl.h Normal file
View File

@ -0,0 +1,14 @@
#ifndef _ROBOT_CONTROL_
#define _ROBOT_CONTROL_
#include "DynamixelHandler.h"
#include "Kinematics.h"
void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate);
void closeRobot(DynamixelHandler& dxlHandler);
void moveJoint(DynamixelHandler& dxlHandler, float q1, float q2, float q3, float qgripper);
void openGripper(DynamixelHandler& dxlHandler);
void closeGripper(DynamixelHandler& dxlHandler);
#endif

10
include/Utils.h Normal file
View File

@ -0,0 +1,10 @@
#ifndef _UTILS_
#define _UTILS_
#include <opencv2/opencv.hpp>
bool estimateRobotCamTransformation(cv::Mat camDataset, cv::Mat robotDataset, cv::Mat& robotCamTransform);
#endif

44
makefile Normal file
View File

@ -0,0 +1,44 @@
all: kinematics dynamixel joint cartesian readwrite calibCamRobot robotControl utils image track pick
g++ -o bin/jointControl lib/jointControl.o lib/Kinematics.o lib/DynamixelHandler.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
g++ -o bin/cartControl lib/cartControl.o lib/Kinematics.o lib/DynamixelHandler.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
g++ -o bin/calibrateCamRobotReferenceFrames lib/Utils.o lib/calibrateCamRobotReferenceFrames.o lib/ImageProcessing.o lib/RobotControl.o lib/ReadWriteFunctions.o lib/Kinematics.o lib/DynamixelHandler.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
g++ -o bin/trackCube lib/Utils.o lib/trackCube.o lib/ImageProcessing.o lib/RobotControl.o lib/ReadWriteFunctions.o lib/Kinematics.o lib/DynamixelHandler.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
g++ -o bin/pickAndPlace lib/Utils.o lib/pickAndPlace.o lib/ImageProcessing.o lib/RobotControl.o lib/ReadWriteFunctions.o lib/Kinematics.o lib/DynamixelHandler.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
dynamixel: src/DynamixelHandler.cpp
g++ -c src/DynamixelHandler.cpp -o lib/DynamixelHandler.o -I./include -I/usr/local/include
kinematics: src/Kinematics.cpp
g++ -c src/Kinematics.cpp -o lib/Kinematics.o -I./include -I/usr/include/opencv4
joint: src/jointControl.cpp
g++ -c src/jointControl.cpp -o lib/jointControl.o -I./include -I/usr/include/opencv4
cartesian: src/cartControl.cpp
g++ -c src/cartControl.cpp -o lib/cartControl.o -I./include -I/usr/include/opencv4
readwrite: src/ReadWriteFunctions.cpp
g++ -c src/ReadWriteFunctions.cpp -o lib/ReadWriteFunctions.o -I./include -I/usr/include/opencv4
calibCamRobot: src/calibrateCamRobotReferenceFrames.cpp
g++ -c src/calibrateCamRobotReferenceFrames.cpp -o lib/calibrateCamRobotReferenceFrames.o -I./include -I/usr/include/opencv4
robotControl: src/RobotControl.cpp
g++ -c src/RobotControl.cpp -o lib/RobotControl.o -I./include -I/usr/include/opencv4
utils: src/Utils.cpp
g++ -c src/Utils.cpp -o lib/Utils.o -I./include -I/usr/include/opencv4
image: src/ImageProcessing.cpp
g++ -c src/ImageProcessing.cpp -o lib/ImageProcessing.o -I./include -I/usr/include/opencv4
track: src/trackCube.cpp
g++ -c src/trackCube.cpp -o lib/trackCube.o -I./include -I/usr/include/opencv4
pick: src/pickAndPlace.cpp
g++ -c src/pickAndPlace.cpp -o lib/pickAndPlace.o -I./include -I/usr/include/opencv4
clean:
rm lib/*.o
rm bin/*

452
src/DynamixelHandler.cpp Normal file
View File

@ -0,0 +1,452 @@
#include "DynamixelHandler.h"
DynamixelHandler::DynamixelHandler():
m_sDeviceName(""), m_fProtocolVersion(0.0), m_i32BaudRate(0),
m_pPacketHandler(nullptr), m_pPortHandler(nullptr),
m_bIsDeviceNameSet(false), m_bIsProtocolVersionSet(false), m_bIsPortOpened(false), m_bIsBaudRateSet(false),
m_ui8DxlError(0), m_i32DxlCommunicationResult(COMM_TX_FAIL)
{
}
DynamixelHandler::~DynamixelHandler()
{
}
int DynamixelHandler::convertJointVelocityToJointCmd(float fJointVelocity)
{
if (fJointVelocity == 0.0f)
return 0;
float a = 0.0f;
float b = 0.0f;
if (fJointVelocity>0)
{
float l_fMaxJointCmd = 1023;
float l_fMinJointCmd = 0;
float l_fMaxJointVelocity = 114 * 60.0f * 2 * M_PI;
float l_fMinJointAngle = 0.0f;
// y = ax + b
a = (l_fMaxJointCmd-l_fMinJointCmd) / (l_fMaxJointVelocity - l_fMinJointAngle);
b = l_fMinJointCmd - a * l_fMinJointAngle;
}
if (fJointVelocity<0)
{
float l_fMaxJointCmd = 2047;
float l_fMinJointCmd = 1024;
float l_fMaxJointVelocity = 0.0f;
float l_fMinJointAngle = -114 * 60.0f * 2 * M_PI;
// y = ax + b
a = (l_fMaxJointCmd-l_fMinJointCmd) / (l_fMaxJointVelocity - l_fMinJointAngle);
b = l_fMinJointCmd - a * l_fMinJointAngle;
}
float jointCmd = a * fJointVelocity + b;
return (int)jointCmd;
}
int DynamixelHandler::convertAngleToJointCmd(float fJointAngle)
{
// y = ax + b
float a = (m_fMaxJointCmd-m_fMinJointCmd) / (m_fMaxJointAngle - m_fMinJointAngle);
float b = m_fMinJointCmd - a * m_fMinJointAngle;
float jointCmd = a * fJointAngle + b;
return (int)jointCmd;
}
float DynamixelHandler::convertJointCmdToAngle(int iJointCmd)
{
// y = ax + b
float a = (m_fMaxJointAngle - m_fMinJointAngle) / (m_fMaxJointCmd-m_fMinJointCmd);
float b = m_fMinJointAngle - a * m_fMinJointCmd;
float jointAngle = a * iJointCmd + b;
return jointAngle;
}
bool DynamixelHandler::openPort()
{
if (m_pPortHandler == nullptr)
{
std::cout << "[ERROR](DynamixelHandler::openPort) m_pPortHandler is null!" << std::endl;
m_bIsPortOpened = false;
return m_bIsPortOpened;
}
if (!m_bIsDeviceNameSet)
{
std::cout << "[ERROR](DynamixelHandler::openPort) m_sDeviceName is not set!" << std::endl;
m_bIsPortOpened = false;
return m_bIsPortOpened;
}
if (m_bIsPortOpened)
{
std::cout << "[WARNING](DynamixelHandler::openPort) port is already opened!" << std::endl;
return m_bIsPortOpened;
}
if (m_pPortHandler->openPort())
{
std::cout << "[INFO](DynamixelHandler::openPort) Succeeded to open the port!" << std::endl;
m_bIsPortOpened = true;
}
else
{
std::cout << "[ERROR](DynamixelHandler::openPort) Failed to open the port!" << std::endl;
m_bIsPortOpened = false;
}
return m_bIsPortOpened;
}
void DynamixelHandler::closePort()
{
if (m_pPortHandler == nullptr)
{
std::cout << "[ERROR](DynamixelHandler::closePort) m_pPortHandler is null!" << std::endl;
m_bIsPortOpened = false;
return;
}
if (!m_bIsPortOpened)
{
std::cout << "[WARNING](DynamixelHandler::openPort) port is already closed!" << std::endl;
return;
}
m_pPortHandler->closePort();
std::cout << "[INFO](DynamixelHandler::closePort) Succeeded to close the port!" << std::endl;
m_bIsPortOpened = false;
}
bool DynamixelHandler::setBaudRate(int i32BaudRate)
{
m_i32BaudRate = i32BaudRate;
if (nullptr != m_pPortHandler)
{
if (m_pPortHandler->setBaudRate(m_i32BaudRate))
{
std::cout << "[INFO](DynamixelHandler::setBaudRate) Succeeded to change the baudrate!" << std::endl;
m_bIsBaudRateSet = true;
}
else
{
std::cout << "[ERROR](DynamixelHandler::setBaudRate) Failed to change the baudrate!" << std::endl;
m_bIsBaudRateSet = false;
}
}
else
{
std::cout << "[ERROR](DynamixelHandler::setBaudRate) m_pPortHandler is null!" << std::endl;
m_bIsBaudRateSet = false;
}
return m_bIsBaudRateSet;
}
void DynamixelHandler::setDeviceName(std::string sDeviceName)
{
m_sDeviceName = sDeviceName;
m_bIsDeviceNameSet = true;
if (nullptr != m_pPortHandler)
{
delete m_pPortHandler;
m_pPortHandler = nullptr;
}
// Initialize PortHandler instance
m_pPortHandler = dynamixel::PortHandler::getPortHandler(m_sDeviceName.c_str());
}
void DynamixelHandler::setProtocolVersion(float fProtocolVersion)
{
m_fProtocolVersion = fProtocolVersion;
m_bIsProtocolVersionSet = true;
if (nullptr != m_pPacketHandler)
{
delete m_pPacketHandler;
m_pPacketHandler = nullptr;
}
m_pPacketHandler = dynamixel::PacketHandler::getPacketHandler(m_fProtocolVersion);
}
bool DynamixelHandler::readCurrentJointPosition(std::vector<float>& vCurrentJointPosition)
{
// Creates a vector of joint position
std::vector<uint16_t> l_vCurrentJointPosition;
// Reads the current joint positions in motor command unit
bool bIsReadSuccessfull = this->readCurrentJointPosition(l_vCurrentJointPosition);
//std::cout << "l_vCurrentJointPosition= " << l_vCurrentJointPosition[0] << ", " << l_vCurrentJointPosition[1] << ", " << l_vCurrentJointPosition[2]<< std::endl;
// q1
vCurrentJointPosition.push_back(ROT_DIRECTION_Q1*convertJointCmdToAngle(l_vCurrentJointPosition[0]));
// q2
vCurrentJointPosition.push_back(ROT_DIRECTION_Q2*convertJointCmdToAngle(l_vCurrentJointPosition[1]));
// q3
vCurrentJointPosition.push_back(ROT_DIRECTION_Q3*convertJointCmdToAngle(l_vCurrentJointPosition[2]));
// qgripper
vCurrentJointPosition.push_back(ROT_DIRECTION_QGRIPPER*convertJointCmdToAngle(l_vCurrentJointPosition[3]));
//std::cout << "vCurrentJointPosition= " << vCurrentJointPosition[0] << ", " << vCurrentJointPosition[1] << ", " << vCurrentJointPosition[2] << ", " << vCurrentJointPosition[3]<< std::endl;
return bIsReadSuccessfull;
}
bool DynamixelHandler::readCurrentJointPosition(std::vector<uint16_t>& vCurrentJointPosition)
{
bool bIsReadSuccessfull = false;
for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++)
{
int dxl_comm_result = COMM_TX_FAIL; // Communication result
uint8_t dxl_error = 0;
uint16_t dxl_present_position = 0;
dxl_comm_result = m_pPacketHandler->read2ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_PRESENT_POSITION, &dxl_present_position, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
//std::cout << "[ERROR] " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl;
bIsReadSuccessfull = false;
}
else if (dxl_error != 0)
{
//std::cout << "[ERROR] " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl;
bIsReadSuccessfull = false;
}
else
{
vCurrentJointPosition.push_back(dxl_present_position);
bIsReadSuccessfull = true;
}
}
return bIsReadSuccessfull;
}
bool DynamixelHandler::sendTargetJointPosition(std::vector<float>& vTargetJointPosition)
{
// Checks if the input vector has the right size
if (vTargetJointPosition.size() != NB_JOINTS)
{
std::cout << "[ERROR] (sendTargetJointPosition) Input vector has not the right size!" << std::endl;
return false;
}
// Creates a vector of motor commands
std::vector<uint16_t> l_vTargetJointPosition;
// q1
l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_Q1*vTargetJointPosition[0]));
// q2
l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_Q2*vTargetJointPosition[1]));
// q3
l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_Q3*vTargetJointPosition[2]));
// qgripper
l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_QGRIPPER*vTargetJointPosition[3]));
//std::cout << "l_vTargetJointPosition= " << l_vTargetJointPosition[0] << ", " << l_vTargetJointPosition[1] << ", " << l_vTargetJointPosition[2]<< ", " << l_vTargetJointPosition[3]<< std::endl;
// call the dxl sendTargetJointPosition
bool bIsSendSuccessfull = this->sendTargetJointPosition(l_vTargetJointPosition);
return bIsSendSuccessfull;
}
bool DynamixelHandler::controlGripper(float f32GripperAngle)
{
// Gets the current joint angles
std::vector<float> l_vCurrentJointPosition;
readCurrentJointPosition(l_vCurrentJointPosition);
//std::cout << "l_vCurrentJointPosition= " << l_vCurrentJointPosition[0] << ", " << l_vCurrentJointPosition[1] << ", " << l_vCurrentJointPosition[2]<< ", " << l_vCurrentJointPosition[3]<< std::endl;
// Checks if the input vector has the right size
if (l_vCurrentJointPosition.size() != NB_JOINTS)
{
std::cout << "[ERROR] (controlGripper) Input vector has not the right size!" << std::endl;
return false;
}
// Rewrites the gripper angle
l_vCurrentJointPosition[3] = f32GripperAngle;
//std::cout << "l_vCurrentJointPosition= " << l_vCurrentJointPosition[0] << ", " << l_vCurrentJointPosition[1] << ", " << l_vCurrentJointPosition[2]<< ", " << l_vCurrentJointPosition[3]<< std::endl;
// Creates a vector of motor commands
std::vector<uint16_t> l_vTargetJointPosition;
// q1
l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_Q1*l_vCurrentJointPosition[0]));
// q2
l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_Q2*l_vCurrentJointPosition[1]));
// q3
l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_Q3*l_vCurrentJointPosition[2]));
// qgripper
l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_QGRIPPER*l_vCurrentJointPosition[3]));
//std::cout << "l_vTargetJointPosition= " << l_vTargetJointPosition[0] << ", " << l_vTargetJointPosition[1] << ", " << l_vTargetJointPosition[2]<< ", " << l_vTargetJointPosition[3]<< std::endl;
// call the dxl sendTargetJointPosition
bool bIsSendSuccessfull = this->sendTargetJointPosition(l_vTargetJointPosition);
return bIsSendSuccessfull;
}
bool DynamixelHandler::sendTargetJointPosition(std::vector<uint16_t>& vTargetJointPosition)
{
bool bIsSendSuccessfull = false;
// checks if the vector size is correct
if (vTargetJointPosition.size() != NB_JOINTS)
{
std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): Size of command vector is not correct: " << vTargetJointPosition.size() << " instead of " << NB_JOINTS << "!" << std::endl;
bIsSendSuccessfull = false;
}
for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++)
{
int dxl_comm_result = COMM_TX_FAIL; // Communication result
uint8_t dxl_error = 0;
uint16_t dxl_present_position = 0;
dxl_comm_result = m_pPacketHandler->write2ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_GOAL_POSITION, vTargetJointPosition[l_joint], &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
//std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl;
bIsSendSuccessfull = false;
}
else if (dxl_error != 0)
{
//std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl;
bIsSendSuccessfull = false;
}
else
{
bIsSendSuccessfull = true;
}
}
return bIsSendSuccessfull;
}
bool DynamixelHandler::sendTargetJointVelocity(std::vector<float>& vTargetJointVelocity)
{
// Checks if the input vector has the right size
if (vTargetJointVelocity.size() != NB_JOINTS)
{
std::cout << "[ERROR] (sendTargetJointVelocity) Input vector has not the right size!" << std::endl;
return false;
}
// Creates a vector of motor commands
std::vector<uint16_t> l_vTargetJointVelocity;
// q1
l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_Q1*vTargetJointVelocity[0]));
// q2
l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_Q2*vTargetJointVelocity[1]));
// q3
l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_Q3*vTargetJointVelocity[2]));
// qgripper
l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_QGRIPPER*vTargetJointVelocity[3]));
std::cout << "l_vTargetJointVelocity= " << l_vTargetJointVelocity[0] << ", " << l_vTargetJointVelocity[1] << ", " << l_vTargetJointVelocity[2] << ", " << l_vTargetJointVelocity[3]<< std::endl;
// call the dxl sendTargetJointPosition
bool bIsSendSuccessfull = this->sendTargetJointVelocity(l_vTargetJointVelocity);
return bIsSendSuccessfull;
}
bool DynamixelHandler::sendTargetJointVelocity(std::vector<uint16_t>& vTargetJointVelocity)
{
bool bIsSendSuccessfull = false;
// checks if the vector size is correct
if (vTargetJointVelocity.size() != NB_JOINTS)
{
std::cout << "[ERROR] (DynamixelHandler::sendTargetJointVelocity): Size of command vector is not correct: " << vTargetJointVelocity.size() << " instead of " << NB_JOINTS << "!" << std::endl;
bIsSendSuccessfull = false;
}
for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++)
{
int dxl_comm_result = COMM_TX_FAIL; // Communication result
uint8_t dxl_error = 0;
uint16_t dxl_present_position = 0;
dxl_comm_result = m_pPacketHandler->write2ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_GOAL_VELOCITY, vTargetJointVelocity[l_joint], &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
//std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl;
bIsSendSuccessfull = false;
}
else if (dxl_error != 0)
{
//std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl;
bIsSendSuccessfull = false;
}
else
{
bIsSendSuccessfull = true;
}
}
return bIsSendSuccessfull;
}
bool DynamixelHandler::enableTorque(bool bEnableTorque)
{
bool bIsSendSuccessfull = false;
for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++)
{
int dxl_comm_result = COMM_TX_FAIL; // Communication result
uint8_t dxl_error = 0;
dxl_comm_result = m_pPacketHandler->write1ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_TORQUE_ENABLE, bEnableTorque, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
//std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl;
bIsSendSuccessfull = false;
}
else if (dxl_error != 0)
{
//std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl;
bIsSendSuccessfull = false;
}
else
{
bIsSendSuccessfull = true;
}
}
return bIsSendSuccessfull;
}
bool DynamixelHandler::setControlMode(int iControlMode)
{
bool bIsSendSuccessfull = false;
for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++)
{
int dxl_comm_result = COMM_TX_FAIL; // Communication result
uint8_t dxl_error = 0;
dxl_comm_result = m_pPacketHandler->write1ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_CONTROL_MODE, iControlMode, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
//std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl;
bIsSendSuccessfull = false;
}
else if (dxl_error != 0)
{
//std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl;
bIsSendSuccessfull = false;
}
else
{
bIsSendSuccessfull = true;
}
}
return bIsSendSuccessfull;
}

316
src/ImageProcessing.cpp Normal file
View File

@ -0,0 +1,316 @@
#include "ImageProcessing.h"
std::vector<std::vector<cv::Point>> imageProcessing(cv::Mat img_original)
{
// displays the original image
cv::imshow( "Original", img_original );
// converts the image to grayscale
cv::Mat img_gray;
cv::cvtColor( img_original, img_gray, cv::COLOR_BGR2GRAY );
// displays the grayscale image
cv::imshow( "Grayscale", img_gray );
cv::Mat img_threshold;
cv::threshold(img_gray, img_threshold, thresholdValue, maxValue, cv::THRESH_BINARY);
if (false)
{
// blurs it
cv::Mat img_blur;
blur( img_gray, img_blur, cv::Size(3,3) );
// displays the blurred image
cv::imshow( "Blur", img_blur );
// applies Canny algorithm on it
cv::Mat canny_output;
cv::Canny( img_blur, canny_output, thresholdValue, maxValue );
// displays the canny image
cv::imshow( "Canny", canny_output );
}
// finds contours on the canny image
std::vector<std::vector<cv::Point> > contours;
std::vector<cv::Vec4i> hierarchy;
cv::findContours( img_threshold, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_NONE );
//cv::findContours( canny_output, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_NONE );
// displays contours on the original image
cv::RNG rng(12345);
//cv::Mat drawing = cv::Mat::zeros( canny_output.size(), CV_8UC3 );
cv::Mat drawing = cv::Mat::zeros( img_threshold.size(), CV_8UC3 );
for( size_t i = 0; i< contours.size(); i++ )
{
cv::Scalar color = cv::Scalar( rng.uniform(0, 256), rng.uniform(0,256), rng.uniform(0,256) );
cv::drawContours( drawing, contours, (int)i, color, 2, cv::LINE_8, hierarchy, 0 );
}
cv::imshow( "Contours", drawing );
return contours;
}
std::vector<std::vector<float>> convertContoursPixel2Mm(std::vector<std::vector<cv::Point>> vContoursInPixel, float theta, float tx, float ty, int imageWidth, int imageHeight)
{
std::vector<std::vector<float>> vContoursInMm;
// pixel / mm ratios
float widthRatio = (float)imageWidth / drawingAreaWidthInMm;
float heigthRatio = (float)imageHeight / drawingAreaHeigthInMm;
float appliedRatio = ceil((widthRatio>=heigthRatio) ? widthRatio : heigthRatio);
// image to real world reference frame transformation
cv::Mat transformationMatrix(3, 3, CV_32FC1);
transformationMatrix.at<float>(0,0) = cos(theta); transformationMatrix.at<float>(0,1) = -sin(theta); transformationMatrix.at<float>(0,2) = tx;
transformationMatrix.at<float>(1,0) = sin(theta); transformationMatrix.at<float>(1,1) = cos(theta); transformationMatrix.at<float>(1,2) = ty;
transformationMatrix.at<float>(2,0) = 0.0; transformationMatrix.at<float>(2,1) = 0.0; transformationMatrix.at<float>(2,2) = 1.0;
// size the output vector with the same number of contours as the input one
vContoursInMm.resize(vContoursInPixel.size());
// fill the output vector by transforming the data
for (int l_contour = 0; l_contour < vContoursInPixel.size(); l_contour++)
{
std::cout << "[INFO] (convertContoursPixel2Mm) contour #" << l_contour << " , # of pts = " << vContoursInPixel[l_contour].size()<< std::endl;
for (int l_point = 0; l_point < vContoursInPixel[l_contour].size(); l_point++)
{
// get the coordinate of the current point in the pixel reference frame
cv::Mat pointCoordinateInPixel(3, 1, CV_32FC1);
pointCoordinateInPixel.at<float>(0,0) = vContoursInPixel[l_contour][l_point].y/ appliedRatio; //!! WATCH OUT Points go (x,y); (width,height)
pointCoordinateInPixel.at<float>(1,0) = vContoursInPixel[l_contour][l_point].x/ appliedRatio;
pointCoordinateInPixel.at<float>(2,0) = 1.0;
// transform it in the real world reference frame
cv::Mat pointCoordinateInMm(3, 1, CV_32FC1);
pointCoordinateInMm = transformationMatrix * pointCoordinateInPixel;
// store the data in the contour in mm
vContoursInMm[l_contour].push_back(pointCoordinateInMm.at<float>(0,0) );
vContoursInMm[l_contour].push_back(pointCoordinateInMm.at<float>(1,0) );
}
}
return vContoursInMm;
}
void sendContours(std::vector<std::vector<cv::Point>> vContoursInPixel, std::vector<std::vector<float>> vContoursInMm, DynamixelHandler& dxlHandler, float L1, float L2, float L3)
{
if (vContoursInPixel.size() == 0 || vContoursInMm.size() == 0)
{
std::cout << "[ERROR](sendContours) Contour vector is empty!" << std::endl;
return;
}
cv::Mat drawing = cv::Mat::zeros( 240, 240, CV_8UC3 );
cv::Scalar color = cv::Scalar( 0, 0, 255 );
cv::imshow( "Current contour", drawing );
float q_pen = deg2rad(-70.0f);
for (int l_contour = 0; l_contour < vContoursInMm.size(); l_contour++)
{
q_pen = deg2rad(-70.0f);
std::cout << "[INFO] (sendContours) contour #" << l_contour << " , # of pts = " << vContoursInMm[l_contour].size()<< std::endl;
for (int l_point = 0; l_point < vContoursInMm[l_contour].size()/2; l_point=l_point+incrementBetweenSamples)
{
std::cout << "l_point #" << l_point << std::endl;
float targetPointX = vContoursInMm[l_contour][2*l_point]/10; // in cm
float targetPointY = vContoursInMm[l_contour][2*l_point+1]/10; // in cm
// Define a constant Z height (in cm) where the drawing surface is located
float targetPointZ = 1.0f; // adjust based on your setup
for (int l_point = 0; l_point < vContoursInMm[l_contour].size() / 2; l_point = l_point + incrementBetweenSamples)
{
std::cout << "l_point #" << l_point << std::endl;
float targetPointX = vContoursInMm[l_contour][2 * l_point] / 10.0f; // in cm
float targetPointY = vContoursInMm[l_contour][2 * l_point + 1] / 10.0f; // in cm
float targetPointI = vContoursInPixel[l_contour][l_point].x;
float targetPointJ = vContoursInPixel[l_contour][l_point].y;
// Plot a dot for visual feedback
cv::circle(drawing, cv::Point(targetPointI, targetPointJ), 1, cv::Scalar(0, 0, 255), cv::FILLED, cv::LINE_8);
cv::imshow("Current contour", drawing);
std::cout << "CurrentPoint = (" << targetPointJ << ", " << targetPointI << ") --> (" << targetPointX << ", " << targetPointY << ")" << std::endl;
// Compute inverse kinematics
std::vector<float> qi = computeInverseKinematics(targetPointX, targetPointY, targetPointZ, L1, L2, L3);
if (qi.size() > 3)
{
std::vector<float> l_vTargetJointPosition;
// q1
l_vTargetJointPosition.push_back(qi[1]);
// q_pen (pen joint angle)
l_vTargetJointPosition.push_back(q_pen);
// q2
l_vTargetJointPosition.push_back(qi[2]);
// Send joint commands
dxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
}
// Lower the pen after the first point
q_pen = deg2rad(-90.0f);
// Add delay to allow smooth movement
cv::waitKey(deltaTBetweenSamples);
}
float targetPointZ;
float targetPointI = vContoursInPixel[l_contour][l_point].x;
float targetPointJ = vContoursInPixel[l_contour][l_point].y;
// Plots a dot at the current point position
cv::circle(drawing, cv::Point(targetPointI ,targetPointJ), 0, cv::Scalar(0,0,255), cv::FILLED, cv::LINE_8);
cv::imshow( "Current contour", drawing );
std::cout<<"CurrentPoint = (" << targetPointJ << "," << targetPointI << ") --> (" << targetPointX << "," << targetPointY << ")" << std::endl;
// IK
std::vector<float> qi = computeInverseKinematics(targetPointX, targetPointY, targetPointZ, L1, L2, L3);
if (qi.size() > 3)
{
std::vector<float> l_vTargetJointPosition;
// q1
l_vTargetJointPosition.push_back(qi[1]);
// fixed joint
l_vTargetJointPosition.push_back(q_pen);
// q2
l_vTargetJointPosition.push_back(qi[2]);
dxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
}
q_pen = deg2rad(-90.0f);
cv::waitKey(deltaTBetweenSamples);
}
}
}
void findObject2DPosition(cv::Mat frame, cv::Mat& frameThresholded, bool bIsImageUndistorted, bool isCamParamsSet, cv::Mat cameraMatrix, cv::Mat distCoeffs, int iLowH, int iLowS, int iLowV, int iHighH, int iHighS, int iHighV, int iStructuralElementSize, int iAreaThresold, int& iLastX, int& iLastY, cv::Mat& imgLines)
{
// undistorts images for both cameras
if (bIsImageUndistorted && isCamParamsSet)
{
cv::Mat temp = frame.clone();
cv::undistort(temp, frame, cameraMatrix, distCoeffs);
}
//Convert the captured frame from BGR to HSV
cv::Mat frameHSV;
cvtColor(frame, frameHSV, cv::COLOR_BGR2HSV);
//Threshold the image based on the trackbar values
inRange(frameHSV, cv::Scalar(iLowH, iLowS, iLowV), cv::Scalar(iHighH, iHighS, iHighV), frameThresholded);
//morphological opening (remove small objects from the foreground)
cv::erode(frameThresholded, frameThresholded, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(iStructuralElementSize, iStructuralElementSize)) );
cv::dilate( frameThresholded, frameThresholded, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(iStructuralElementSize, iStructuralElementSize)) );
//morphological closing (fill small holes in the foreground)
cv::dilate( frameThresholded, frameThresholded, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(iStructuralElementSize, iStructuralElementSize)) );
cv::erode(frameThresholded, frameThresholded, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(iStructuralElementSize, iStructuralElementSize)) );
// calculates the moments of the thresholded image
cv::Moments oMoments = moments(frameThresholded);
double dM01 = oMoments.m01;
double dM10 = oMoments.m10;
double dArea = oMoments.m00;
// if the area <= iAreaThresold, considers that the there are no object in the image and it's because of the noise, the area is not zero
if (dArea > iAreaThresold)
{
// calculates the position of the ball
int posX = dM10 / dArea;
int posY = dM01 / dArea;
if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0)
{
// draw a red line from the previous point to the current point
line(imgLines, cv::Point(posX, posY), cv::Point(iLastX, iLastY), cv::Scalar(0,0,255), 2);
}
// stores the current position for enxt frame
iLastX = posX;
iLastY = posY;
}
}
void findObject2DPosition(cv::Mat frame, cv::Mat& frameThresholded, int iLowH, int iLowS, int iLowV, int iHighH, int iHighS, int iHighV, int iStructuralElementSize, int iAreaThresold, int& iLastX, int& iLastY, cv::Mat& imgLines)
{
//Convert the captured frame from BGR to HSV
cv::Mat frameHSV;
cvtColor(frame, frameHSV, cv::COLOR_BGR2HSV);
//Threshold the image based on the trackbar values
inRange(frameHSV, cv::Scalar(iLowH, iLowS, iLowV), cv::Scalar(iHighH, iHighS, iHighV), frameThresholded);
//morphological opening (remove small objects from the foreground)
cv::erode(frameThresholded, frameThresholded, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(iStructuralElementSize, iStructuralElementSize)) );
cv::dilate( frameThresholded, frameThresholded, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(iStructuralElementSize, iStructuralElementSize)) );
//morphological closing (fill small holes in the foreground)
cv::dilate( frameThresholded, frameThresholded, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(iStructuralElementSize, iStructuralElementSize)) );
cv::erode(frameThresholded, frameThresholded, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(iStructuralElementSize, iStructuralElementSize)) );
// calculates the moments of the thresholded image
cv::Moments oMoments = moments(frameThresholded);
double dM01 = oMoments.m01;
double dM10 = oMoments.m10;
double dArea = oMoments.m00;
// if the area <= iAreaThresold, considers that the there are no object in the image and it's because of the noise, the area is not zero
if (dArea > iAreaThresold)
{
// calculates the position of the ball
int posX = dM10 / dArea;
int posY = dM01 / dArea;
if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0)
{
// draw a red line from the previous point to the current point
line(imgLines, cv::Point(posX, posY), cv::Point(iLastX, iLastY), cv::Scalar(0,0,255), 2);
}
// stores the current position for enxt frame
iLastX = posX;
iLastY = posY;
}
}
void findObject3DPosition(int iLastXR, int iLastYR, int iLastXL, int iLastYL, cv::Mat frameThresholded, cv::Mat Q, double& x, double& y, double& z)
{
// estimates average position between the left and right images
float v = (float)iLastYL;
float u = (float)iLastXL;
float disparity = abs((float)iLastXR - (float)iLastXL);
//std::cout << "disp= " << disparity << std::endl;
// projects 2D position to 3D world
cv::Mat_<double> vec_tmp(4,1);
if (u > 0 & u < frameThresholded.cols & v > 0 & v < frameThresholded.rows)
{
vec_tmp(0)=(double)u;
vec_tmp(1)=(double)v;
vec_tmp(2)=(double)disparity;
vec_tmp(3)=1.0;
vec_tmp = Q*vec_tmp;
vec_tmp /= vec_tmp(3);
}
// saves cartesian coordinates
x = vec_tmp(0);
y = vec_tmp(1);
z = vec_tmp(2);
}

190
src/Kinematics.cpp Normal file
View File

@ -0,0 +1,190 @@
#include "Kinematics.h"
float deg2rad(float angle)
{
return angle/180.0*M_PI;
}
float rad2deg(float angle)
{
return angle*180.0/M_PI;
}
std::vector<float> computeForwardKinematics(float q1, float q2, float q3, float L1, float L2, float L3)
{
q2 *= -1.0;
q3 *= -1.0;
float z = L2 * cos(q2) + L3 * cos(q2+q3) + L1;
float xp = L2 * sin(q2) + L3 * sin(q2+q3);
float x = xp * cos(q1);
float y = xp * sin(q1);
//std::cout << "[INFO] Forward Kinematics : (q1, q2, q3)->(x, y, z) = (" << rad2deg(q1) << ", " << rad2deg(q2) << ", " << rad2deg(q3) << ")->(" << x << ", " << y << ", " << z << ")" << std::endl;
std::vector<float> X;
X.push_back(x);
X.push_back(y);
X.push_back(z);
return X;
}
std::vector<float> computeInverseKinematics(float x, float y, float z, float L1, float L2, float L3)
{
std::vector<float> qi;
// Determines q1
float q1 = atan2(y, x);
// Determines cos(q3) to find out the number of solutions
float xp = sqrt(x*x + y*y);
float zc = z - L1;
//std::cout << "zc= " << zc<< std::endl;
float cos_q3 = (zc*zc+xp*xp-(L2*L2+L3*L3)) / (2.0 * L2 * L3);
//std::cout << "[INFO] cos_q3= " << cos_q3 << std::endl;
if (cos_q3 >1 | cos_q3 <-1)
{
qi.push_back(0.0);
std::cout << "[INFO] Inverse Kinematics: No solution!" << std::endl;
}
else if (cos_q3 == 1)
{
qi.push_back(1.0);
float q2 = -M_PI/2.0 + atan2(zc, xp);
float q3 = 0;
std::cout << "[INFO] Inverse Kinematics: One solution: (x, y, z)->(q1, q2, q3) = (" << x << ", " << y << ", " << z << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ", " << rad2deg(q3) << ")" << std::endl;
qi.push_back(q1);
qi.push_back(q2);
qi.push_back(q3);
}
else if (cos_q3 == -1)
{
qi.push_back(1.0);
float q2 = -M_PI/2.0 + atan2(zc, xp);
float q3 = M_PI;
std::cout << "[INFO] Inverse Kinematics: One solution: (x, y, z)->(q1, q2, q3) = (" << x << ", " << y << ", " << z << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ", " << rad2deg(q3) << ")" << std::endl;
qi.push_back(q1);
qi.push_back(q2);
qi.push_back(q3);
}
else
{
qi.push_back(2.0);
std::cout << "[INFO] Inverse Kinematics: Two solutions: "<< std::endl;
float q3 = acos(cos_q3);
float q2 = (float)(-M_PI/2.0 + atan2(zc, xp) - atan2(L3*sin(q3), L2+L3*cos_q3));
std::cout << "\t(x, y)->(q1, q2, q3) = (" << x << ", " << y << ", " << z << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ", " << rad2deg(q3) << ")" << std::endl;
qi.push_back(q1);
qi.push_back(q2);
qi.push_back(q3);
q3 = -acos(cos_q3);
q2 = (float)(-M_PI/2.0 + atan2(zc, xp) - atan2(L3*sin(q3), L2+L3*cos_q3));
std::cout << "\t(x, y)->(q1, q2, q3) = (" << x << ", " << y << ", " << z << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ", " << rad2deg(q3) << ")" << std::endl;
qi.push_back(q1);
qi.push_back(q2);
qi.push_back(q3);
}
return qi;
}
std::vector<float> computeDifferentialKinematics(float q1, float q2, float q3, float L1, float L2, float L3)
{
std::vector<float> jacobian;
float j11 = -sin(q1)*(L2*sin(q2) + L3*sin(q2+q3));
float j12 = L2*cos(q1)*cos(q2)+L3*cos(q1)*cos(q2+q3);
float j13 = L3*cos(q1)*cos(q2+q3);
float j21 = cos(q1)*(L2*sin(q2)+L3*sin(q2+q3));
float j22 = L2*sin(q1)*cos(q2)+L3*sin(q1)*cos(q2+q3);
float j23 = L3*sin(q1)*cos(q2+q3);
float j31 = 0;
float j32 = -L2*sin(q2)-L3*sin(q2+q3);
float j33 = -L3*sin(q2+q3);
jacobian.push_back(j11); jacobian.push_back(j12); jacobian.push_back(j13);
jacobian.push_back(j21); jacobian.push_back(j22); jacobian.push_back(j23);
jacobian.push_back(j31); jacobian.push_back(j32); jacobian.push_back(j33);
return jacobian;
}
int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold)
{
int rank = -1;
cv::Mat1f oJacobianMatrix(3, 3);
if (vJacobianMatrix.size() == 9)
{
// Converts the Jacobian matrix from std::vector to cv::Mat
oJacobianMatrix.at<float>(0, 0) = vJacobianMatrix[0];
oJacobianMatrix.at<float>(0, 1) = vJacobianMatrix[1];
oJacobianMatrix.at<float>(0, 2) = vJacobianMatrix[2];
oJacobianMatrix.at<float>(1, 0) = vJacobianMatrix[3];
oJacobianMatrix.at<float>(1, 1) = vJacobianMatrix[4];
oJacobianMatrix.at<float>(1, 2) = vJacobianMatrix[5];
oJacobianMatrix.at<float>(2, 0) = vJacobianMatrix[6];
oJacobianMatrix.at<float>(2, 1) = vJacobianMatrix[7];
oJacobianMatrix.at<float>(2, 2) = vJacobianMatrix[8];
std::cout << "=====Jacobian Matrix=====" << std::endl;
std::cout << "[ " << oJacobianMatrix.at<float>(0,0) << ", " << oJacobianMatrix.at<float>(0,1) << ", " << oJacobianMatrix.at<float>(0,2) << " ]" << std::endl;
std::cout << "[ " << oJacobianMatrix.at<float>(1,0) << ", " << oJacobianMatrix.at<float>(1,1) << ", " << oJacobianMatrix.at<float>(1,2)<< " ]" << std::endl;
std::cout << "[ " << oJacobianMatrix.at<float>(2,0) << ", " << oJacobianMatrix.at<float>(2,1) << ", " << oJacobianMatrix.at<float>(2,2)<< " ]" << std::endl;
// Computes the determinant of the Jacobian matrix
float determinant = abs(-vJacobianMatrix[7]*(vJacobianMatrix[0] * vJacobianMatrix[5] - vJacobianMatrix[3]*vJacobianMatrix[2])+vJacobianMatrix[8]*(vJacobianMatrix[0] * vJacobianMatrix[4] - vJacobianMatrix[3]*vJacobianMatrix[3]));
std::cout << "=====Determinant of the Jacobian matrix=====" << std::endl << determinant << std::endl;
// Computes SVD
cv::Mat1f w, u, vt;
cv::SVD::compute(oJacobianMatrix, w, u, vt);
// Finds non zero singular values
cv::Mat1f nonZeroSingularValues = w/w.at<float>(0,0) > threshold;
// Counts the number of non zero singular values
rank = cv::countNonZero(nonZeroSingularValues);
std::cout << "=====Rank of the Jacobian matrix=====" << std::endl << rank << " / " << oJacobianMatrix.rows << std::endl;
// Determines the inverse of the Jacobian matrix
cv::Mat oJacobianInverse = oJacobianMatrix.inv();
std::cout << "=====Inverse of the Jacobian Matrix=====" << std::endl;
std::cout << "[ " << oJacobianInverse.at<float>(0,0) << ", " << oJacobianInverse.at<float>(0,1)<< ", " << oJacobianInverse.at<float>(0,2) << " ]" << std::endl;
std::cout << "[ " << oJacobianInverse.at<float>(1,0) << ", " << oJacobianInverse.at<float>(1,1) << ", " << oJacobianInverse.at<float>(1,2)<< " ]" << std::endl;
std::cout << "[ " << oJacobianInverse.at<float>(2,0) << ", " << oJacobianInverse.at<float>(2,1) << ", " << oJacobianInverse.at<float>(2,2)<< " ]" << std::endl;
}
else
std::cout << "[ERROR] Jacobian matrix has a size of "<< vJacobianMatrix.size() << " instead of 4" << std::endl;
return rank;
}
cv::Mat computeInverseJacobianMatrix(std::vector<float> vJacobianMatrix)
{
cv::Mat1f oJacobianMatrix(2, 2);
cv::Mat oJacobianInverse;
if (vJacobianMatrix.size() == 4)
{
// Converts the Jacobian matrix from std::vector to cv::Mat
oJacobianMatrix.at<float>(0, 0) = vJacobianMatrix[0];
oJacobianMatrix.at<float>(0, 1) = vJacobianMatrix[1];
oJacobianMatrix.at<float>(1, 0) = vJacobianMatrix[2];
oJacobianMatrix.at<float>(1, 1) = vJacobianMatrix[3];
std::cout << "=====Jacobian Matrix=====" << std::endl;
std::cout << "[ " << oJacobianMatrix.at<float>(0,0) << ", " << oJacobianMatrix.at<float>(0,1) << " ]" << std::endl;
std::cout << "[ " << oJacobianMatrix.at<float>(1,0) << ", " << oJacobianMatrix.at<float>(1,1) << " ]" << std::endl;
// Determines the inverse of the Jacobian matrix
cv::invert(oJacobianMatrix, oJacobianInverse, cv::DECOMP_SVD);
//oJacobianInverse = oJacobianMatrix.inv();
std::cout << "=====Inverse of the Jacobian Matrix=====" << std::endl;
std::cout << "[ " << oJacobianInverse.at<float>(0,0) << ", " << oJacobianInverse.at<float>(0,1) << " ]" << std::endl;
std::cout << "[ " << oJacobianInverse.at<float>(1,0) << ", " << oJacobianInverse.at<float>(1,1) << " ]" << std::endl;
}
else
std::cout << "[ERROR] Jacobian matrix has a size of "<< vJacobianMatrix.size() << " instead of 4" << std::endl;
return oJacobianInverse;
}

220
src/ReadWriteFunctions.cpp Normal file
View File

@ -0,0 +1,220 @@
#include "ReadWriteFunctions.h"
bool readCamRobotCalibrationParameters(std::string filename, cv::Mat& robotCamTransform)
{
cv::FileStorage fs(filename, cv::FileStorage::READ);
if (!fs.isOpened())
{
std::cout << "[ERROR] Could not open (READ) the camera/robot calibration parameter file storage: " << filename << " !"<< std::endl;
return false;
}
fs["robotCamTransform"] >> robotCamTransform;
// releases the reader
fs.release();
return true;
}
bool readMonoCameraCalibParameters(std::string filename, cv::Mat &camMatrix, cv::Mat & distCoeffs)
{
cv::FileStorage fs(filename, cv::FileStorage::READ);
if (!fs.isOpened())
{
std::cout << "[ERROR] Could not open (READ) the mono camera calibration parameter file storage: " << filename << " !"<< std::endl;
return false;
}
fs["camera_matrix"] >> camMatrix;
fs["distortion_coefficients"] >> distCoeffs;
// releases the reader
fs.release();
return true;
}
bool readStereoCameraParameters(std::string filename, cv::Mat &camLMatrix, cv::Mat & distLCoeffs, cv::Mat &camRMatrix, cv::Mat & distRCoeffs, cv::Mat &stereoMapL_x, cv::Mat &stereoMapL_y, cv::Mat &stereoMapR_x, cv::Mat &stereoMapR_y, cv::Mat &Q)
{
cv::FileStorage fs(filename, cv::FileStorage::READ);
if (!fs.isOpened())
{
std::cout << "[ERROR] Could not open (READ) the stereo camera parameter file storage: " << filename << " !"<< std::endl;
return false;
}
fs["Left_Camera_Matrix"] >> camLMatrix;
fs["Left_Camera_Distortion"] >> distLCoeffs;
fs["Right_Camera_Matrix"] >> camRMatrix;
fs["Right_Camera_Distortion"] >> distRCoeffs;
fs["Left_Stereo_Map_x"] >> stereoMapL_x;
fs["Left_Stereo_Map_y"] >> stereoMapL_y;
fs["Right_Stereo_Map_x"] >> stereoMapR_x;
fs["Right_Stereo_Map_y"] >> stereoMapR_y;
fs["Q"] >> Q;
// releases the reader
fs.release();
return true;
}
bool readDisparityParameters(std::string filename, int &numDisparities, int &blockSize , int &preFilterType, int &preFilterSize, int &preFilterCap, int &minDisparity , int &textureThreshold, int &uniquenessRatio, int &speckleRange, int &speckleWindowSize, int &disp12MaxDiff)
{
cv::FileStorage fs(filename, cv::FileStorage::READ);
if (!fs.isOpened())
{
std::cout << "[ERROR] Could not open (READ) the disparity parameter file storage: " << filename << " !"<< std::endl;
return false;
}
fs["numDisparities"] >> numDisparities;
fs["blockSize"] >> blockSize;
fs["preFilterType"] >> preFilterType;
fs["preFilterSize"] >> preFilterSize;
fs["preFilterCap"] >> preFilterCap;
fs["minDisparity"] >> minDisparity;
fs["textureThreshold"] >> textureThreshold;
fs["uniquenessRatio"] >> uniquenessRatio;
fs["speckleRange"] >> speckleRange;
fs["speckleWindowSize"] >> speckleWindowSize;
fs["disp12MaxDiff"] >> disp12MaxDiff;
// releases the reader
fs.release();
return true;
}
bool readColorParameters(std::string filename, int& iLowH, int& iHighH, int& iLowS, int& iHighS, int& iLowV, int& iHighV)
{
cv::FileStorage fs(filename, cv::FileStorage::READ);
if (!fs.isOpened())
{
std::cout << "[ERROR] Could not open (READ) the color parameter file storage: " << filename << " !"<< std::endl;
return false;
}
fs["lowH"] >> iLowH;
fs["highH"] >> iHighH;
fs["lowS"] >> iLowS;
fs["highS"] >> iHighS;
fs["lowV"] >> iLowV;
fs["highV"] >> iHighV;
// releases the reader
fs.release();
return true;
}
bool writeMonoCameraParameters(std::string filename, cv::Mat camMatrix, cv::Mat distCoeffs)
{
cv::FileStorage fs(filename, cv::FileStorage::WRITE);
if (!fs.isOpened())
{
std::cout << "[ERROR] Could not open (WRITE) the mono camera parameter file storage: " << filename << " !"<< std::endl;
return false;
}
fs << "camera_matrix" << camMatrix;
fs << "distortion_coefficients" << distCoeffs;
// releases the writer
fs.release();
return true;
}
bool writeColorParameters(std::string filename, int iLowH, int iHighH, int iLowS, int iHighS, int iLowV, int iHighV)
{
cv::FileStorage fs(filename, cv::FileStorage::WRITE);
if (!fs.isOpened())
{
std::cout << "[ERROR] Could not open (WRITE) the color parameter file storage: " << filename << " !"<< std::endl;
return false;
}
fs << "lowH" << iLowH;
fs << "highH" << iHighH;
fs << "lowS" << iLowS;
fs << "highS" << iHighS;
fs << "lowV" << iLowV;
fs << "highV" << iHighV;
// releases the writer
fs.release();
return true;
}
bool writeStereoCameraParameters(std::string filename, cv::Mat camLMatrix, cv::Mat distLCoeffs, cv::Mat camRMatrix, cv::Mat distRCoeffs, cv::Mat stereoMapL_x, cv::Mat stereoMapL_y, cv::Mat stereoMapR_x, cv::Mat stereoMapR_y, cv::Mat Q)
{
cv::FileStorage fs(filename, cv::FileStorage::WRITE);
if (!fs.isOpened())
{
std::cout << "[ERROR] Could not open (WRITE) the stereo camera parameter file storage: " << filename << " !"<< std::endl;
return false;
}
fs << "Left_Camera_Matrix" << camLMatrix;
fs << "Left_Camera_Distortion" << distLCoeffs;
fs << "Right_Camera_Matrix" << camRMatrix;
fs << "Right_Camera_Distortion" << distRCoeffs;
fs << "Left_Stereo_Map_x" << stereoMapL_x;
fs << "Left_Stereo_Map_y" << stereoMapL_y;
fs << "Right_Stereo_Map_x" << stereoMapR_x;
fs << "Right_Stereo_Map_y" << stereoMapR_y;
fs << "Q" << Q;
// releases the writer
fs.release();
return true;
}
bool writeDisparityParameters(std::string filename, int numDisparities, int blockSize , int preFilterType, int preFilterSize, int preFilterCap, int textureThreshold, int uniquenessRatio, int speckleRange, int speckleWindowSize, int disp12MaxDiff, int minDisparity )
{
cv::FileStorage fs(filename, cv::FileStorage::WRITE);
if (!fs.isOpened())
{
std::cout << "[ERROR] Could not open (WRITE) the disparity parameter file storage: " << filename << " !"<< std::endl;
return false;
}
fs << "numDisparities" << numDisparities;
fs << "blockSize" << blockSize;
fs << "preFilterType" << preFilterType;
fs << "preFilterSize" << preFilterSize;
fs << "preFilterCap" << preFilterCap;
fs << "minDisparity" << minDisparity;
fs << "textureThreshold" << textureThreshold;
fs << "uniquenessRatio" << uniquenessRatio;
fs << "speckleRange" << speckleRange;
fs << "speckleWindowSize" << speckleWindowSize;
fs << "disp12MaxDiff" << disp12MaxDiff;
// releases the writer
fs.release();
return true;
}
bool writeCamRobotCalibrationParameters(std::string filename, cv::Mat robotCamTransform)
{
cv::FileStorage fs(filename, cv::FileStorage::WRITE);
if (!fs.isOpened())
{
std::cout << "[ERROR] Could not open (WRITE) the camera/robot calibration parameter file storage: " << filename << " !"<< std::endl;
return false;
}
fs << "robotCamTransform" << robotCamTransform;
// releases the writer
fs.release();
return true;
}

41
src/RobotControl.cpp Normal file
View File

@ -0,0 +1,41 @@
#include "RobotControl.h"
#define GRIPPER_OPEN -60.0
#define GRIPPER_CLOSE 20.0
void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
{
std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
dxlHandler.setDeviceName(portName);
dxlHandler.setProtocolVersion(protocol);
dxlHandler.openPort();
dxlHandler.setBaudRate(baudRate);
dxlHandler.enableTorque(true);
std::cout << std::endl;
}
void closeRobot(DynamixelHandler& dxlHandler)
{
dxlHandler.enableTorque(false);
dxlHandler.closePort();
}
void moveJoint(DynamixelHandler& dxlHandler, float q1, float q2, float q3, float qgripper)
{
std::vector<float> vTargetJointPosition;
vTargetJointPosition.push_back(q1);
vTargetJointPosition.push_back(q2);
vTargetJointPosition.push_back(q3);
vTargetJointPosition.push_back(qgripper);
dxlHandler.sendTargetJointPosition(vTargetJointPosition);
}
void openGripper(DynamixelHandler& dxlHandler)
{
dxlHandler.controlGripper(deg2rad(GRIPPER_OPEN));
}
void closeGripper(DynamixelHandler& dxlHandler)
{
dxlHandler.controlGripper(deg2rad(GRIPPER_CLOSE));
}

70
src/Utils.cpp Normal file
View File

@ -0,0 +1,70 @@
#include "Utils.h"
bool estimateRobotCamTransformation(cv::Mat camDataset, cv::Mat robotDataset, cv::Mat& robotCamTransform)
{
// checks if datasets have the same size
if (camDataset.dims != robotDataset.dims || camDataset.size != robotDataset.size)
{
std::cout << "[ERROR] cam and robot datasets have different sizes!" << std::endl;
return false;
}
// computes the mean point of both datasets
cv::Mat meanCamDataset, meanRobotDataset;
cv::reduce(camDataset, meanCamDataset, 1, cv::REDUCE_AVG);
cv::reduce(robotDataset, meanRobotDataset, 1, cv::REDUCE_AVG);
//std::cout << "meanCamDataset= " << meanCamDataset << std::endl;
//std::cout << "meanRobotDataset= " << meanRobotDataset << std::endl;
// normalises the datasets by removing the average
cv::Mat camDatasetNorm = camDataset;
cv::Mat robotDatasetNorm = robotDataset;
cv::Size size = camDataset.size();
for (int x=0; x < size.width; ++x)
{
cv::Rect rect(x, 0, 1, size.height);
camDatasetNorm(rect) = camDataset(rect) - meanCamDataset;
robotDatasetNorm(rect) = robotDataset(rect) - meanRobotDataset;
}
//std::cout << "camDatasetNorm= " << camDatasetNorm << std::endl;
//std::cout << "robotDatasetNorm= " << robotDatasetNorm << std::endl;
// computes the covariance matrix H
cv::Mat H;
H = camDatasetNorm* robotDatasetNorm.t();
//std::cout << "H= " << H << std::endl;
// computes the SVD of the covariance matrix
cv::Mat W, U, Vt;
cv::SVD::compute(H, W, U, Vt);
// computes the rotation matrix
cv::Mat Ut = U.t();
cv::Mat V = Vt.t();
cv::Mat R = V * Ut;
//std::cout << "R= " << R << std::endl;
// forces a right hand coordinate system
if (determinant(R) < 0)
{
for (int l_coord=0; l_coord < 3; l_coord++)
R.at<double>(l_coord,2) *= -1;
}
std::cout << "R= " << R << std::endl;
cv::Mat Rvec;
cv::Rodrigues(R, Rvec);
std::cout << "Rvec= " << Rvec * 57.32<< std::endl;
// estimates the translation matrix
cv::Mat t ;
t = -R * meanCamDataset + meanRobotDataset;
std::cout << "t= " << t << std::endl;
// fills the matrix with R and t
R(cv::Range(0,3), cv::Range(0,3)).copyTo(robotCamTransform(cv::Range(0,3), cv::Range(0,3)));
t(cv::Range(0,3), cv::Range(0,1)).copyTo(robotCamTransform(cv::Range(0,3), cv::Range(3,4)));
return true;
}

393
src/calibrateCamRobotReferenceFrames.cpp Normal file
View File

@ -0,0 +1,393 @@
#include <chrono>
#include <thread>
#include <iostream>
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <time.h>
#include <opencv2/opencv.hpp>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <opencv2/calib3d.hpp>
#include "ReadWriteFunctions.h"
#include "ImageProcessing.h"
#include "Kinematics.h"
#include "DynamixelHandler.h"
#include "RobotControl.h"
#include "Utils.h"
#define STEREO_PARAMS_FILENAME "./data/stereo_params.xml"
#define COLOR_PARAMS_FILENAME "./data/color_params.xml"
#define CAM_ROBOT_CALIB_FILENAME "./data/camrobot_params.xml"
#define FPS 30.0
#define STRUCTURAL_ELEMENTS_SIZE 5
#define CAM_LEFT_INDEX 2
#define CAM_RIGHT_INDEX 0
#define AREA_THRESOLD 1000
#define RESOLUTION_MAX 800
#define ROBOT_PORT_NAME "/dev/ttyUSB0"
#define ROBOT_PROTOCOL 2.0
#define ROBOT_BAUDRATE 1000000
#define ROBOT_L1 3.0
#define ROBOT_L2 7.0
#define ROBOT_L3 11.0
#define ROBOT_Q1 0.0
#define ROBOT_Q2 -60.0
#define ROBOT_Q3 0.0
#define ROBOT_GRIPPER 0.0
#define CUBE_OFFSET 1.5
#define CALIB_NB_POINTS 5
// global vars
std::string sStereoCameraParamFilename = STEREO_PARAMS_FILENAME;
std::string sColorParamFilename = COLOR_PARAMS_FILENAME;
int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE;
int iAreaThresold = AREA_THRESOLD;
int iCamLIndex = CAM_LEFT_INDEX;
int iCamRIndex = CAM_RIGHT_INDEX;
float fFPS = FPS;
bool processArgs(int argc, char** argv)
{
int opt;
while ((opt = getopt (argc, argv, ":i:c:f:s:l:r:a:")) != -1)
{
switch (opt)
{
case 'i':
sStereoCameraParamFilename = optarg;
break;
case 'c':
sColorParamFilename = optarg;
break;
case 'f':
fFPS = atof(optarg);
break;
case 's':
iStructuralElementSize = atoi(optarg);
break;
case 'l':
iCamLIndex = atoi(optarg);
break;
case 'r':
iCamRIndex = atoi(optarg);
break;
case 'a':
iAreaThresold = atoi(optarg);
break;
case '?':
if (optopt == 'i' || optopt == 'd' || optopt == 'c' || optopt == 'f' || optopt == 's' || optopt == 'l' || optopt == 'r' || optopt == 'a')
fprintf (stderr, "Option -%c requires an argument.\n", optopt);
else if (isprint (optopt))
fprintf (stderr, "Unknown option `-%c'.\n", optopt);
else
fprintf (stderr, "Unknown option character `\\x%x'.\n", optopt);
return false;
default:
abort ();
}
}
return true;
}
int main(int argc, char** argv)
{
// random values
srand(time(NULL));
// robot vars
DynamixelHandler _oDxlHandler;
std::string _robotDxlPortName = ROBOT_PORT_NAME;
float _robotDxlProtocol = ROBOT_PROTOCOL;
int _robotDxlBaudRate = ROBOT_BAUDRATE;
float L1 = ROBOT_L1;
float L2 = ROBOT_L2;
float L3 = ROBOT_L3;
float q1 = ROBOT_Q1;
float q2 = deg2rad(ROBOT_Q2);
float q3 = ROBOT_Q3;
float qgripper = ROBOT_GRIPPER;
// color cars
int iLowH, iHighH, iLowS, iHighS, iLowV, iHighV;
// stereocam params
cv::Mat cameraMatrixL, distCoeffsL, cameraMatrixR, distCoeffsR;
cv::Mat stereoMapL_x, stereoMapL_y, stereoMapR_x, stereoMapR_y, Q;
// output file
std::string sCamRobotCalibrationFilename = CAM_ROBOT_CALIB_FILENAME;
std::cout << "========INITIALISATION PHASE========" <<std::endl;
// updates main parameters from arguments
std::cout << "[INFO] Process args... ";
bool isArgSet = processArgs(argc, argv);
if (!isArgSet)
{
std::cout << std::endl;
std::cout << "\t[ERROR] Args could not be set!" << std::endl;
return 1;
}
std::cout << "OK!" << std::endl;
// initializes the robot
std::cout << "[INFO] Initialize the robot" << std::endl;
initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate);
// reads color parameters from the file storage
std::cout << "[INFO] Read color parameters... ";
bool _isColorParamsSet = readColorParameters(sColorParamFilename, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV);
if (!_isColorParamsSet)
{
std::cout << std::endl;
std::cout << "\t[ERROR] Color parameters could not be loaded!" << std::endl;
return 2;
}
std::cout << "OK!" << std::endl;
// reads camera intrinsic & extrinsic parameters
std::cout << "[INFO] Read Stereo parameters... ";
bool _isCamParamsSet = readStereoCameraParameters(sStereoCameraParamFilename, cameraMatrixL, distCoeffsL, cameraMatrixR, distCoeffsR, stereoMapL_x, stereoMapL_y, stereoMapR_x, stereoMapR_y, Q);
if (!_isCamParamsSet)
{
std::cout << std::endl;
std::cout << "\t[ERROR] Stereo camera parameters could not be loaded!" << std::endl;
return 3;
}
std::cout << "OK!" << std::endl;
// creates a camera grabber for each camera left/right
std::cout << "[INFO] Opening camera videostreams...";
cv::VideoCapture camL(iCamLIndex, cv::CAP_V4L2), camR(iCamRIndex, cv::CAP_V4L2);
// changes image resolution to maximum i.e. 1920x1080 if possible
camL.set(cv::CAP_PROP_FRAME_HEIGHT, RESOLUTION_MAX); camL.set(cv::CAP_PROP_FRAME_WIDTH, RESOLUTION_MAX);
camR.set(cv::CAP_PROP_FRAME_HEIGHT, RESOLUTION_MAX); camR.set(cv::CAP_PROP_FRAME_WIDTH, RESOLUTION_MAX);
// checks if the left camera was successfully opened
if (!camL.isOpened()) // if not success, exit program
{
std::cout << std::endl;
std::cout << "\t[ERROR] Could not open the left camera!" << std::endl;
return 4;
}
// checks if the right camera was successfully opened
if (!camR.isOpened()) // if not success, exit program
{
std::cout << std::endl;
std::cout << "\t[ERROR] Could not open the right camera!" << std::endl;
return 5;
}
std::cout << "OK!" << std::endl;
// gets image resolution for info
std::cout << "\t-->Left camera resolution: " << camL.get(cv::CAP_PROP_FRAME_WIDTH) << "x" << camL.get(cv::CAP_PROP_FRAME_HEIGHT) << std::endl;
std::cout << "\t-->Right camera resolution: " << camR.get(cv::CAP_PROP_FRAME_WIDTH) << "x" << camR.get(cv::CAP_PROP_FRAME_HEIGHT) << std::endl;
// inits display
cv::namedWindow("Original", cv::WINDOW_NORMAL);
cv::resizeWindow("Original", 1280, 480);
cv::namedWindow("Thresholded", cv::WINDOW_NORMAL);
cv::resizeWindow("Thresholded", 1280, 480);
// inits previous x,y location of the ball
int iLastXR = -1; int iLastYR = -1;
int iLastXL = -1; int iLastYL = -1;
// captures a temporary image from the camera
cv::Mat imgTmp;
camL.read(imgTmp);
// creates a black image with the size as the camera output
cv::Mat imgLinesL = cv::Mat::zeros( imgTmp.size(), CV_8UC3 );
cv::Mat imgLinesR = cv::Mat::zeros( imgTmp.size(), CV_8UC3 );
std::cout << "========PLACE THE CUBE IN THE ROBOT GRIPPER========" <<std::endl;
// sends the target joint values received as args
std::cout << "[INFO] Move to the initial position"<< std::endl;
moveJoint(_oDxlHandler, q1, q2, q3, qgripper);
std::this_thread::sleep_for(std::chrono::milliseconds(2000)); // waits 2s
// opens the gripper
std::cout << "[INFO] Opening the gripper"<< std::endl;
openGripper(_oDxlHandler);
std::this_thread::sleep_for(std::chrono::milliseconds(2000)); // waits 2s
// places the cube at the gripper level
std::cout << "*** Place the cube and press any key to close the gripper ***"<< std::endl;
int key;
std::cin >> key;
std::cout << "[INFO] Closing the gripper"<< std::endl;
closeGripper(_oDxlHandler);
std::this_thread::sleep_for(std::chrono::milliseconds(2000)); // waits 2s
std::cout << "========DATA COLLECTION PHASE========" <<std::endl;
// main loop launched every FPS
bool bIsTrainingSetFull = false;
int i32NbPoints = 0;
cv::Mat camDataset = cv::Mat(3, CALIB_NB_POINTS, CV_64F);
cv::Mat robotDataset = cv::Mat(3, CALIB_NB_POINTS, CV_64F);
while (!bIsTrainingSetFull)
{
cv::Mat frameL, frameR;
cv::Mat frameThresholdedL, frameThresholdedR;
cv::Mat frameNiceL, frameNiceR;
// reads a new frame from left/right cameras
bool bLCamSuccess = camL.read(frameL);
bool bRCamSuccess = camR.read(frameR);
// checks if a new frame was grabbed for both cameras
if (!bLCamSuccess || !bRCamSuccess) //if not success, break loop
{
std::cout << "[WARNING] Could not read both frames from video streams" << std::endl;
break;
}
// applies stereo image rectification on both images
cv::remap(frameL, frameNiceL, stereoMapL_x, stereoMapL_y, cv::INTER_LANCZOS4, cv::BORDER_CONSTANT, 0);
cv::remap(frameR, frameNiceR, stereoMapR_x, stereoMapR_y, cv::INTER_LANCZOS4, cv::BORDER_CONSTANT, 0);
// finds the object position in pixels
findObject2DPosition(frameNiceL, frameThresholdedL, iLowH, iLowS, iLowV, iHighH, iHighS, iHighV, iStructuralElementSize, iAreaThresold, iLastXL, iLastYL, imgLinesL);
findObject2DPosition(frameNiceR, frameThresholdedR, iLowH, iLowS, iLowV, iHighH, iHighS, iHighV, iStructuralElementSize, iAreaThresold, iLastXR, iLastYR, imgLinesR);
// draws markers
cv::drawMarker(frameThresholdedL, cv::Point(iLastXL,iLastYL), cv::Scalar(0, 255, 255), cv::MARKER_CROSS, 10, 1);
cv::drawMarker(frameThresholdedR, cv::Point(iLastXR,iLastYR), cv::Scalar(0, 255, 255), cv::MARKER_CROSS, 10, 1);
// finds the object 3D position
double x, y, z;
findObject3DPosition( iLastXR, iLastYR, iLastXL, iLastYL, frameThresholdedL, Q, x, y, z);
//std::cout << "XYZ_cm= (" << x/10 << ", " << y/10 << ", " << z/10 <<")"<< std::endl;
// displays the thresholded L/R frames
cv::Mat tempThres;
cv::hconcat(frameThresholdedL, frameThresholdedR, tempThres);
cv::imshow("Thresholded", tempThres); //show the thresholded image
cv::putText(frameNiceL, "Press g to grab the frame", cv::Point(50, 50), 2, 1, cv::Scalar(0, 200, 0), 2);
cv::putText(frameNiceL, "Press n to change position", cv::Point(50, 100), 2, 1, cv::Scalar(0, 200, 0), 2);
// shows the original image with the tracking (red) lines
frameNiceL = frameNiceL + imgLinesL;
frameNiceR = frameNiceR + imgLinesR;
cv::Mat temp;
cv::hconcat(frameNiceL, frameNiceR, temp);
imshow("Original", temp);
// waits for awhile depending on the FPS value
char key = (char)cv::waitKey(1000.0/fFPS);
// checks if ESC was pressed to exit
if (key == 27) // if 'esc' key is pressed, break loop
{
std::cout << "[INFO] Esc key is pressed by user -> Shutting down without calibrating!" << std::endl;
return 1;
}
// moves the robot to the next position
if (key == 'n')
{
std::cout << "[INFO] Move to next robot position"<< std::endl;
q1 = deg2rad(rand() % 60 -30);
q2 = deg2rad(rand() % 20 -10+ ROBOT_Q2);
q3 = deg2rad(rand() % 30 -15);
qgripper = 20;
moveJoint(_oDxlHandler, q1, q2, q3, qgripper);
}
// grabs the current position
if (key == 'g' )
{
// end-effector position in {camera}
camDataset.at<double>(0, i32NbPoints) = x/10; // in cm
camDataset.at<double>(1, i32NbPoints) = y/10;
camDataset.at<double>(2, i32NbPoints) = z/10 + CUBE_OFFSET;
// end-effector position in {robot}
std::vector<float> xyz = computeForwardKinematics(q1, q2, q3, L1, L2, L3);
robotDataset.at<double>(0, i32NbPoints) = (double)xyz[0];
robotDataset.at<double>(1, i32NbPoints) = (double)xyz[1];
robotDataset.at<double>(2, i32NbPoints) = (double)xyz[2];
i32NbPoints++;
std::cout << i32NbPoints << " / " << CALIB_NB_POINTS << std::endl;
//std::cout << "camDataset = " << camDataset << std::endl;
//std::cout << "robotDataset = " << robotDataset << std::endl;
}
if (i32NbPoints >= CALIB_NB_POINTS)
{
std::cout << "[INFO] Datasets are full!" << std::endl;
bIsTrainingSetFull = true;
}
}
std::cout << "========CALIBRATION PHASE========" <<std::endl;
if (camDataset.cols == CALIB_NB_POINTS && robotDataset.cols == CALIB_NB_POINTS)
{
//std::cout << "camDataset = " << camDataset << std::endl;
//std::cout << "robotDataset = " << robotDataset << std::endl;
cv::Mat robotCamTransform = cv::Mat::eye(4,4, CV_64F);
estimateRobotCamTransformation(camDataset, robotDataset, robotCamTransform);
std::cout << "robotCamTransform = " << robotCamTransform << std::endl;
// saves the disparity params in a yaml file
std::cout << "[INFO] Save the result in a yaml file... ";
bool bIsCamRobotCalibrationSaved = writeCamRobotCalibrationParameters(sCamRobotCalibrationFilename, robotCamTransform);
// checks if the disparity parameters were successfully read
if (!bIsCamRobotCalibrationSaved)
{
std::cout << std::endl;
std::cout << "\t[ERROR] Cam/Robot parameters could not be saved!" << std::endl;
return 4;
}
std::cout << "OK!" << std::endl;
}
else
{
std::cout << "[ERROR] Calibration cannot be performed because recorded data is corrupted... ";
}
std::cout << "========CLOSING PHASE========" <<std::endl;
// Closes robot connection
std::cout << "[INFO] Closing the robot connection"<< std::endl;
closeRobot(_oDxlHandler);
// releases video streams
camL.release();
camR.release();
// destroys all windows
cv::destroyAllWindows();
return 0;
}

84
src/cartControl.cpp Normal file
View File

@ -0,0 +1,84 @@
#include <chrono>
#include <thread>
#include "Kinematics.h"
#include "DynamixelHandler.h"
DynamixelHandler _oDxlHandler;
std::string _robotDxlPortName = "/dev/ttyUSB0";
float _robotDxlProtocol = 2.0;
int _robotDxlBaudRate = 1000000;
void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
{
std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
dxlHandler.setDeviceName(portName);
dxlHandler.setProtocolVersion(protocol);
dxlHandler.openPort();
dxlHandler.setBaudRate(baudRate);
dxlHandler.enableTorque(true);
std::cout << std::endl;
}
void closeRobot(DynamixelHandler& dxlHandler)
{
dxlHandler.enableTorque(false);
dxlHandler.closePort();
}
int main(int argc, char** argv)
{
if (argc == 7)
{
// Retrieves the args and stores them into variables
float L1 = atof(argv[1]); // in cm
float L2 = atof(argv[2]); // in cm
float L3 = atof(argv[3]); // in cm
float x = atof(argv[4]); // in cm
float y = atof(argv[5]); // in cm
float z = atof(argv[6]); // in cm
float qgripper = deg2rad(0); // in rad
// Initializes the robot
initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate);
// Computes IK
std::vector<float> qi = computeInverseKinematics(x, y, z, L1, L2, L3);
// Computes FK
computeForwardKinematics(qi[1], qi[2], qi[3], atof(argv[1]), atof(argv[2]), atof(argv[3]));
// Computes Differential Kinematics
std::vector<float> vJacobianMatrix = computeDifferentialKinematics(qi[1], qi[2], qi[3], L1, L2, L3);
int rank = computeJacobianMatrixRank(vJacobianMatrix, 0.1);
// Sends the target joint values received only if there is at least a solution
if (qi.size() >= 3)
{
std::cout << "IK has " << qi[0] << " solutions" << std::endl;
std::cout << "qi = (" << qi[1] << ", " << qi[2] << ", " << qi[3] << ")" << std::endl;
std::vector<float> vTargetJointPosition;
vTargetJointPosition.push_back(qi[4]);
vTargetJointPosition.push_back(qi[5]);
vTargetJointPosition.push_back(qi[6]);
vTargetJointPosition.push_back(qgripper);
_oDxlHandler.sendTargetJointPosition(vTargetJointPosition);
}
// Waits 2s
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// Closes robot connection
_oDxlHandler.closePort();
}
else
{
std::cout << "[WARNING] Cartesian control" << std::endl;
std::cout << ">> cartControl L1(cm) L2(cm) L3(cm) x(cm) y(cm) z(cm)"<< std::endl;
std::cout << ">> cartControl 5.5 7 12 0 0 24.5"<< std::endl;
}
return 0;
}

89
src/jointControl.cpp Normal file
View File

@ -0,0 +1,89 @@
#include <chrono>
#include <thread>
#include "Kinematics.h"
#include "DynamixelHandler.h"
// Global variables
DynamixelHandler _oDxlHandler;
std::string _robotDxlPortName = "/dev/ttyUSB0";
float _robotDxlProtocol = 2.0;
int _robotDxlBaudRate = 1000000;
void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
{
std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
dxlHandler.setDeviceName(portName);
dxlHandler.setProtocolVersion(protocol);
dxlHandler.openPort();
dxlHandler.setBaudRate(baudRate);
dxlHandler.enableTorque(true);
std::cout << std::endl;
}
void closeRobot(DynamixelHandler& dxlHandler)
{
dxlHandler.enableTorque(false);
dxlHandler.closePort();
}
int main(int argc, char** argv)
{
if (argc == 8)
{
// Retrieves the args and stores them into variables
float L1 = atof(argv[1]); // in cm
float L2 = atof(argv[2]); // in cm
float L3 = atof(argv[3]); // in cm
float q1 = deg2rad(atof(argv[4])); // in rad
float q2 = deg2rad(atof(argv[5])); // in rad
float q3 = deg2rad(atof(argv[6])); // in rad
float qgripper = deg2rad(atof(argv[7])); // in rad
// Initializes the robot
initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate);
// Computes FK
computeForwardKinematics(q1, q2, q3, L1, L2, L3);
// Sends the target joint values received as args
std::vector<float> vTargetJointPosition;
vTargetJointPosition.push_back(q1);
vTargetJointPosition.push_back(q2);
vTargetJointPosition.push_back(q3);
vTargetJointPosition.push_back(qgripper);
_oDxlHandler.sendTargetJointPosition(vTargetJointPosition);
//std::cout << "vTargetJointPosition= " << vTargetJointPosition[0] << ", " << vTargetJointPosition[1] << ", " << vTargetJointPosition[2]<< std::endl;
// Waits 2s
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
std::cout << "controlGripper(0)"<< std::endl;
_oDxlHandler.controlGripper(deg2rad(0));
// Waits 2s
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
std::cout << "controlGripper(-40)"<< std::endl;
_oDxlHandler.controlGripper(deg2rad(-40));
// Waits 2s
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
std::cout << "controlGripper(0)"<< std::endl;
_oDxlHandler.controlGripper(deg2rad(0));
// Waits 2s
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// Closes robot connection
_oDxlHandler.closePort();
}
else
{
std::cout << "[WARNING] Joint control"<< std::endl;
std::cout << ">> jointControl L1(cm) L2(cm) L3(cm) q1(deg) q2(deg) q3(deg) qgripper(deg)"<< std::endl;
std::cout << ">> jointControl 5.5 7 12 0 0 0 0"<< std::endl;
}
return 0;
}

111
src/pickAndPlace.cpp Normal file
View File

@ -0,0 +1,111 @@
#include <opencv2/opencv.hpp>
#include <vector>
#include <iostream>
// Assuming a robot interface exists to control the arm
// Replace this with your actual robot interface
class RobotArm {
public:
void moveTo(const cv::Point3f& position) {
// Simulated movement to a target position
std::cout << "Moving to position: (" << position.x << ", " << position.y << ", " << position.z << ")\n";
}
void grab() {
// Simulate grabbing the cube
std::cout << "Grabbing cube\n";
}
void release() {
// Simulate releasing the cube
std::cout << "Releasing cube\n";
}
};
// Function to detect cubes of the specified color (blue)
std::vector<cv::Point3f> detectCubes(const cv::Mat& image, const cv::Scalar& colorLower, const cv::Scalar& colorUpper) {
std::vector<cv::Point3f> cubes;
// Convert image to HSV
cv::Mat hsvImage;
cv::cvtColor(image, hsvImage, cv::COLOR_BGR2HSV);
// Create a mask for the specified color range
cv::Mat mask;
cv::inRange(hsvImage, colorLower, colorUpper, mask);
// Find contours in the mask
std::vector<std::vector<cv::Point>> contours;
cv::findContours(mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
for (const auto& contour : contours) {
if (cv::contourArea(contour) > 100) { // Ignore small contours
// Get the bounding box of the contour
cv::Rect boundingBox = cv::boundingRect(contour);
// Assuming z = 0 for simplicity, adjust this as needed for your setup
cubes.push_back(cv::Point3f(boundingBox.x + boundingBox.width / 2.0f,
boundingBox.y + boundingBox.height / 2.0f,
0.0f));
}
}
return cubes;
}
// Function to grab a cube
void grabCube(RobotArm& robot, const cv::Point3f& position) {
robot.moveTo(position);
robot.grab();
}
// Function to place the cube at a new position
void placeCube(RobotArm& robot, const cv::Point3f& position) {
robot.moveTo(position);
robot.release();
}
// Main function for pick and place operation
void pickAndPlace(cv::Mat& image, const cv::Scalar& colorLower, const cv::Scalar& colorUpper, const cv::Point3f& targetPosition) {
// Detect all cubes in the workspace
std::vector<cv::Point3f> cubes = detectCubes(image, colorLower, colorUpper);
// Sort cubes by their y-coordinate (assuming y is the vertical axis)
std::sort(cubes.begin(), cubes.end(), [](const cv::Point3f& a, const cv::Point3f& b) {
return a.y < b.y;
});
// Create a robot arm instance
RobotArm robot;
// Iterate over the detected cubes, pick them, and place them on top of each other
for (size_t i = 0; i < cubes.size(); ++i) {
grabCube(robot, cubes[i]);
// Calculate the new position to place the cube (stack on top of the previous cube)
cv::Point3f newPosition = cv::Point3f(targetPosition.x, targetPosition.y, targetPosition.z + (i * 0.05f)); // Adjust height increment as needed
placeCube(robot, newPosition);
}
}
int main() {
// Read an image from the camera or workspace
cv::Mat image = cv::imread("workspace_image.jpg");
if (image.empty()) {
std::cerr << "Error: Could not open or find the image!" << std::endl;
return -1;
}
// Define the HSV color range for blue (adjust this range based on your lighting conditions)
cv::Scalar colorLower(100, 150, 0); // Lower bound of blue color
cv::Scalar colorUpper(140, 255, 255); // Upper bound of blue color
// Target position where cubes will be stacked (e.g., starting position)
cv::Point3f targetPosition(0.5f, 0.5f, 0.0f); // Example position (x, y, z)
// Execute the pick and place operation
pickAndPlace(image, colorLower, colorUpper, targetPosition);
return 0;
}

334
src/trackCube.cpp Normal file
View File

@ -0,0 +1,334 @@
#include <chrono>
#include <thread>
#include <iostream>
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <time.h>
#include <opencv2/opencv.hpp>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <opencv2/calib3d.hpp>
#include "ReadWriteFunctions.h"
#include "ImageProcessing.h"
#include "Kinematics.h"
#include "DynamixelHandler.h"
#include "RobotControl.h"
#include "Utils.h"
#define STEREO_PARAMS_FILENAME "./data/stereo_params.xml"
#define COLOR_PARAMS_FILENAME "./data/color_params.xml"
#define CAM_ROBOT_CALIB_FILENAME "./data/camrobot_params.xml"
#define FPS 30.0
#define STRUCTURAL_ELEMENTS_SIZE 5
#define CAM_LEFT_INDEX 2
#define CAM_RIGHT_INDEX 0
#define AREA_THRESOLD 1000
#define RESOLUTION_MAX 800
#define ROBOT_PORT_NAME "/dev/ttyUSB0"
#define ROBOT_PROTOCOL 2.0
#define ROBOT_BAUDRATE 1000000
#define ROBOT_L1 3.0
#define ROBOT_L2 7.0
#define ROBOT_L3 11.0
#define ROBOT_Q1 0.0
#define ROBOT_Q2 -60.0
#define ROBOT_Q3 0.0
#define ROBOT_GRIPPER 0.0
// global vars
std::string sStereoCameraParamFilename = STEREO_PARAMS_FILENAME;
std::string sColorParamFilename = COLOR_PARAMS_FILENAME;
int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE;
int iAreaThresold = AREA_THRESOLD;
int iCamLIndex = CAM_LEFT_INDEX;
int iCamRIndex = CAM_RIGHT_INDEX;
float fFPS = FPS;
bool processArgs(int argc, char** argv)
{
int opt;
while ((opt = getopt (argc, argv, ":i:c:f:s:l:r:a:")) != -1)
{
switch (opt)
{
case 'i':
sStereoCameraParamFilename = optarg;
break;
case 'c':
sColorParamFilename = optarg;
break;
case 'f':
fFPS = atof(optarg);
break;
case 's':
iStructuralElementSize = atoi(optarg);
break;
case 'l':
iCamLIndex = atoi(optarg);
break;
case 'r':
iCamRIndex = atoi(optarg);
break;
case 'a':
iAreaThresold = atoi(optarg);
break;
case '?':
if (optopt == 'i' || optopt == 'd' || optopt == 'c' || optopt == 'f' || optopt == 's' || optopt == 'l' || optopt == 'r' || optopt == 'a')
fprintf (stderr, "Option -%c requires an argument.\n", optopt);
else if (isprint (optopt))
fprintf (stderr, "Unknown option `-%c'.\n", optopt);
else
fprintf (stderr, "Unknown option character `\\x%x'.\n", optopt);
return false;
default:
abort ();
}
}
return true;
}
int main(int argc, char** argv)
{
// random values
srand(time(NULL));
// robot vars
DynamixelHandler _oDxlHandler;
std::string _robotDxlPortName = ROBOT_PORT_NAME;
float _robotDxlProtocol = ROBOT_PROTOCOL;
int _robotDxlBaudRate = ROBOT_BAUDRATE;
float L1 = ROBOT_L1;
float L2 = ROBOT_L2;
float L3 = ROBOT_L3;
float q1 = ROBOT_Q1;
float q2 = deg2rad(ROBOT_Q2);
float q3 = ROBOT_Q3;
float qgripper = ROBOT_GRIPPER;
// color cars
int iLowH, iHighH, iLowS, iHighS, iLowV, iHighV;
// stereocam params
cv::Mat cameraMatrixL, distCoeffsL, cameraMatrixR, distCoeffsR;
cv::Mat stereoMapL_x, stereoMapL_y, stereoMapR_x, stereoMapR_y, Q;
// robot/cam params
cv::Mat robotCamTransform;
// output file
std::string sCamRobotCalibrationFilename = CAM_ROBOT_CALIB_FILENAME;
std::cout << "========INITIALISATION PHASE========" <<std::endl;
// updates main parameters from arguments
std::cout << "[INFO] Process args... ";
bool isArgSet = processArgs(argc, argv);
if (!isArgSet)
{
std::cout << std::endl;
std::cout << "\t[ERROR] Args could not be set!" << std::endl;
return 1;
}
std::cout << "OK!" << std::endl;
// initializes the robot
std::cout << "[INFO] Initialize the robot" << std::endl;
initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate);
// reads color parameters from the file storage
std::cout << "[INFO] Read color parameters... ";
bool _isColorParamsSet = readColorParameters(sColorParamFilename, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV);
if (!_isColorParamsSet)
{
std::cout << std::endl;
std::cout << "\t[ERROR] Color parameters could not be loaded!" << std::endl;
return 2;
}
std::cout << "OK!" << std::endl;
// reads camera intrinsic & extrinsic parameters
std::cout << "[INFO] Read Stereo parameters... ";
bool _isCamParamsSet = readStereoCameraParameters(sStereoCameraParamFilename, cameraMatrixL, distCoeffsL, cameraMatrixR, distCoeffsR, stereoMapL_x, stereoMapL_y, stereoMapR_x, stereoMapR_y, Q);
if (!_isCamParamsSet)
{
std::cout << std::endl;
std::cout << "\t[ERROR] Stereo camera parameters could not be loaded!" << std::endl;
return 3;
}
std::cout << "OK!" << std::endl;
// reads camera/robot calibration parameters
bool _isRobotCamParamsSet = readCamRobotCalibrationParameters(sCamRobotCalibrationFilename, robotCamTransform);
if (!_isRobotCamParamsSet)
{
std::cout << std::endl;
std::cout << "\t[ERROR] Robot/Camera calibration parameters could not be loaded!" << std::endl;
return 3;
}
std::cout << "OK!" << std::endl;
// creates a camera grabber for each camera left/right
std::cout << "[INFO] Opening camera videostreams...";
cv::VideoCapture camL(iCamLIndex, cv::CAP_V4L2), camR(iCamRIndex, cv::CAP_V4L2);
// changes image resolution to maximum i.e. 1920x1080 if possible
camL.set(cv::CAP_PROP_FRAME_HEIGHT, RESOLUTION_MAX); camL.set(cv::CAP_PROP_FRAME_WIDTH, RESOLUTION_MAX);
camR.set(cv::CAP_PROP_FRAME_HEIGHT, RESOLUTION_MAX); camR.set(cv::CAP_PROP_FRAME_WIDTH, RESOLUTION_MAX);
// checks if the left camera was successfully opened
if (!camL.isOpened()) // if not success, exit program
{
std::cout << std::endl;
std::cout << "\t[ERROR] Could not open the left camera!" << std::endl;
return 4;
}
// checks if the right camera was successfully opened
if (!camR.isOpened()) // if not success, exit program
{
std::cout << std::endl;
std::cout << "\t[ERROR] Could not open the right camera!" << std::endl;
return 5;
}
std::cout << "OK!" << std::endl;
// gets image resolution for info
std::cout << "\t-->Left camera resolution: " << camL.get(cv::CAP_PROP_FRAME_WIDTH) << "x" << camL.get(cv::CAP_PROP_FRAME_HEIGHT) << std::endl;
std::cout << "\t-->Right camera resolution: " << camR.get(cv::CAP_PROP_FRAME_WIDTH) << "x" << camR.get(cv::CAP_PROP_FRAME_HEIGHT) << std::endl;
// inits display
cv::namedWindow("Original", cv::WINDOW_NORMAL);
cv::resizeWindow("Original", 1280, 480);
cv::namedWindow("Thresholded", cv::WINDOW_NORMAL);
cv::resizeWindow("Thresholded", 1280, 480);
// inits previous x,y location of the ball
int iLastXR = -1; int iLastYR = -1;
int iLastXL = -1; int iLastYL = -1;
// captures a temporary image from the camera
cv::Mat imgTmp;
camL.read(imgTmp);
// creates a black image with the size as the camera output
cv::Mat imgLinesL = cv::Mat::zeros( imgTmp.size(), CV_8UC3 );
cv::Mat imgLinesR = cv::Mat::zeros( imgTmp.size(), CV_8UC3 );
std::cout << "========PLACE THE CUBE IN THE ROBOT GRIPPER========" <<std::endl;
// sends the target joint values received as args
std::cout << "[INFO] Move to the initial position"<< std::endl;
moveJoint(_oDxlHandler, q1, q2, q3, qgripper);
std::this_thread::sleep_for(std::chrono::milliseconds(2000)); // waits 2s
std::cout << "========TRACKING PHASE========" <<std::endl;
// main loop launched every FPS
while (true)
{
cv::Mat frameL, frameR;
cv::Mat frameThresholdedL, frameThresholdedR;
cv::Mat frameNiceL, frameNiceR;
// reads a new frame from left/right cameras
bool bLCamSuccess = camL.read(frameL);
bool bRCamSuccess = camR.read(frameR);
// checks if a new frame was grabbed for both cameras
if (!bLCamSuccess || !bRCamSuccess) //if not success, break loop
{
std::cout << "[WARNING] Could not read both frames from video streams" << std::endl;
break;
}
// applies stereo image rectification on both images
cv::remap(frameL, frameNiceL, stereoMapL_x, stereoMapL_y, cv::INTER_LANCZOS4, cv::BORDER_CONSTANT, 0);
cv::remap(frameR, frameNiceR, stereoMapR_x, stereoMapR_y, cv::INTER_LANCZOS4, cv::BORDER_CONSTANT, 0);
// finds the object position in pixels
findObject2DPosition(frameNiceL, frameThresholdedL, iLowH, iLowS, iLowV, iHighH, iHighS, iHighV, iStructuralElementSize, iAreaThresold, iLastXL, iLastYL, imgLinesL);
findObject2DPosition(frameNiceR, frameThresholdedR, iLowH, iLowS, iLowV, iHighH, iHighS, iHighV, iStructuralElementSize, iAreaThresold, iLastXR, iLastYR, imgLinesR);
// draws markers
cv::drawMarker(frameThresholdedL, cv::Point(iLastXL,iLastYL), cv::Scalar(0, 255, 255), cv::MARKER_CROSS, 10, 1);
cv::drawMarker(frameThresholdedR, cv::Point(iLastXR,iLastYR), cv::Scalar(0, 255, 255), cv::MARKER_CROSS, 10, 1);
// finds the object 3D position in the {camera} frame
double x, y, z;
findObject3DPosition( iLastXR, iLastYR, iLastXL, iLastYL, frameThresholdedL, Q, x, y, z);
// determines the object 3D position in the {robot} frame
cv::Mat_<double> ptsCameraFrame(4,1);
ptsCameraFrame(0) = x/10;
ptsCameraFrame(1) = y/10;
ptsCameraFrame(2) = z/10;
ptsCameraFrame(3) = 1;
std::cout << "XYZ_{camera}= " << ptsCameraFrame<< std::endl;
cv::Mat_<double> ptsRobotFrame(4,1);
ptsRobotFrame = robotCamTransform*ptsCameraFrame;
std::cout << "XYZ_{robot}= " << ptsRobotFrame << std::endl;
// applies IK
std::vector<float> qi = computeInverseKinematics(ptsRobotFrame(0), ptsRobotFrame(1), ptsRobotFrame(2), L1, L2, L3);
if (qi[0] == 2)
{
std::cout << "qi2= (" << qi[4] << ", " << qi[5] << ", " << qi[6] << ")" << std::endl;
moveJoint(_oDxlHandler, qi[4] , qi[5], qi[6], 0);
}
if (qi[0] == 1)
{
std::cout << "qi1= (" << qi[1] << ", " << qi[2] << ", " << qi[3] << ")" << std::endl;
moveJoint(_oDxlHandler, qi[1] , qi[2], qi[3], 0);
}
// displays the thresholded L/R frames
cv::Mat tempThres;
cv::hconcat(frameThresholdedL, frameThresholdedR, tempThres);
cv::imshow("Thresholded", tempThres); //show the thresholded image
// shows the original image with the tracking (red) lines
frameNiceL = frameNiceL + imgLinesL;
frameNiceR = frameNiceR + imgLinesR;
cv::Mat temp;
cv::hconcat(frameNiceL, frameNiceR, temp);
imshow("Original", temp);
// waits for awhile depending on the FPS value
char key = (char)cv::waitKey(1000.0/fFPS);
// checks if ESC was pressed to exit
if (key == 27) // if 'esc' key is pressed, break loop
{
std::cout << "[INFO] Esc key is pressed by user -> Shutting down without calibrating!" << std::endl;
break;
}
}
std::cout << "========CLOSING PHASE========" <<std::endl;
// Closes robot connection
std::cout << "[INFO] Closing the robot connection"<< std::endl;
closeRobot(_oDxlHandler);
// releases video streams
camL.release();
camR.release();
// destroys all windows
cv::destroyAllWindows();
return 0;
}