update

Author: MoreInfoy

.gitignore

@@ -0,0 +1,14 @@
+CMakeLists.txt.user
+CMakeCache.txt
+CMakeFiles
+CMakeScripts
+Testing
+Makefile
+cmake_install.cmake
+install_manifest.txt
+compile_commands.json
+CTestTestfile.cmake
+build/
+install/
+log/
+_deps

.vscode/settings.json

@@ -0,0 +1,75 @@
+{
+    "files.associations": {
+        "ostream": "cpp",
+        "iostream": "cpp",
+        "iosfwd": "cpp",
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "csignal": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "strstream": "cpp",
+        "bit": "cpp",
+        "*.tcc": "cpp",
+        "bitset": "cpp",
+        "cfenv": "cpp",
+        "chrono": "cpp",
+        "compare": "cpp",
+        "complex": "cpp",
+        "concepts": "cpp",
+        "condition_variable": "cpp",
+        "cstdint": "cpp",
+        "deque": "cpp",
+        "forward_list": "cpp",
+        "list": "cpp",
+        "map": "cpp",
+        "set": "cpp",
+        "string": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "functional": "cpp",
+        "iterator": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "ratio": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "fstream": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "numbers": "cpp",
+        "semaphore": "cpp",
+        "shared_mutex": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "stop_token": "cpp",
+        "streambuf": "cpp",
+        "thread": "cpp",
+        "cinttypes": "cpp",
+        "typeindex": "cpp",
+        "typeinfo": "cpp",
+        "variant": "cpp"
+    }
+}

LICENSE

@@ -1,21 +1,29 @@
-MIT License
+BSD 3-Clause License
 
-Copyright (c) 2021 NimaPng
+Copyright (c) 2022, YANG Shunpeng
+All rights reserved.
 
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
 
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
 
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its
+   contributors may be used to endorse or promote products derived from
+   this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 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
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

Poplar/src/LIPM/LIPM_MPC.cpp

@@ -141,7 +141,7 @@ void LIPM_MPC::run() {
     cin << -_c_lb, _c_ub;
     solver_state = eiquadprog_solver.solve_quadprog(_H, _g, Mat(0, _C.cols()), Vec(0), Cin, cin,
                                                     _uOptimal);
-    printf("solver state: %d\n", solver_state);
+    // printf("solver state: %d\n", solver_state);
     if (solver_state != eiquadprog::solvers::EIQUADPROG_FAST_OPTIMAL) {
         //      throw runtime_error("TaskSpaceControl::solve() qp failed, related data has been saved in qp_failed.txt");
         std::cerr << "LIPM_MPC::solve() qp failed" << endl;
@@ -296,7 +296,7 @@ Vec LIPM_MPC::forceDistribute(Poplar::Index ith_horizon, Vec3 pos, Vec3 linear_v
 //    solver_state = eiquadprog_solver_fd.solve_quadprog(H, g, Ce, -ce, CI, cI, force_optimal);
     solver_state = eiquadprog_solver_fd.solve_quadprog(H, g, Mat::Zero(0, 3 * n_dims), Vec::Zero(0), CI, cI,
                                                        force_optimal);
-    printf("solver state: %d\n", solver_state);
+    // printf("solver state: %d\n", solver_state);
     if (solver_state != eiquadprog::solvers::EIQUADPROG_FAST_OPTIMAL) {
         throw runtime_error("LIPM_MPC::forceDistribute() qp failed, related data has been saved in qp_failed.txt");
         std::cerr << "LIPM_MPC::forceDistribute() qp failed" << endl;

Poplar/src/SRGB_MPC/SRGB_MPC.cpp

@@ -220,7 +220,7 @@ void SRGB_MPC_IMPL::solve(Scalar t_now) {
     cin << -_lb, _ub;
     solver_state = eiquadprog_solver.solve_quadprog(_H, _g, Mat(0, 3 * _n_contact), Vec(0), Cin, cin,
                                                     _optimalContactForce);
-    printf("solver state: %d\n", solver_state);
+    // printf("solver state: %d\n", solver_state);
     if (solver_state != eiquadprog::solvers::EIQUADPROG_FAST_OPTIMAL) {
         //      throw runtime_error("TaskSpaceControl::solve() qp failed, related data has been saved in qp_failed.txt");
         std::cerr << "SRGB_MPC::solve() qp failed" << endl;

Poplar/src/TaskSpaceControl/TaskSpaceControl.cpp

@@ -176,7 +176,7 @@ void TaskSpaceControl::solve() {
     cin << -c_lb, c_ub;
     ce0 = -ce;
     solver_state = eiquadprog_solver.solve_quadprog(H, g, Ce, ce0, Cin, cin, optimal_u);
-    printf("solver state: %d\n", solver_state);
+    // printf("solver state: %d\n", solver_state);
     if (solver_state != eiquadprog::solvers::EIQUADPROG_FAST_OPTIMAL) {
         saveAllData("qp_failed.txt");
         throw runtime_error("TaskSpaceControl::solve() qp failed, related data has been saved in qp_failed.txt");

README.md

@@ -1 +1,45 @@
 # Digit Simulation and Motion Control
+
+## Introduction
+
+This package provides insight into various algorithms used for motion planning of humanoid robots, such as foothold placement, COM trajectory optimization, and controller design for closed-chain robots. By utilizing this package, users can gain knowledge about these complex algorithms and their implementation.
+
+<div align="center">
+    <img src="./digit.png">
+</div>
+
+## Advantage
+As far as the author is aware, this package represents the first opens-source complete solution for motion planning and control of humanoid robot with closed chains. It is hoped that this package will be useful to those looking to learn about motion planning and control algorithms. Please note that some of the algorithms included in this package have not yet been published and are subject to copyright protection. (Note: forward veolocity beyond **4.0m/s** in simulator [MuJoCo](https://mujoco.org/))
+
+https://drive.google.com/file/d/1cLD83HytvqX6r0aE3TzPF23wfX0IMfzw/view?usp=sharing
+
+### clone this project
+
+```
+# Open a new terminal
+git clone https://github.com/pianist1234567i/Digit.git
+```
+
+### Build this pakcage
+```
+cd Digit & mkdir build
+```
+
+```
+cd build & cmake ..
+```
+
+```
+make -j4
+```
+
+### Run package
+```
+sh ../run.sh
+```
+
+# Reference
+
+[1] Yang, Shunpeng, et al. "Improved Task Space Locomotion Controller for a Quadruped Robot with Parallel Mechanisms." 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2022.
+
+[2] Featherstone, Roy. Rigid body dynamics algorithms. Springer, 2014.

Simulation/CMakeLists.txt

@@ -10,4 +10,4 @@ include_directories(include)
 
 link_directories(${THIS_COM}/Simulation/lib/)
 
-target_link_libraries(MuJoCoSim  mujoco210 GL glew glfw rt pthread lcm)
+target_link_libraries(MuJoCoSim  mujoco210 GL glew glfw rt pthread)