go1 description and keyboard input

2024-09-11 11:32:54 +08:00 · 2024-09-11 11:32:54 +08:00 · a494a4e854
parent 0cc499210a
commit a494a4e854
38 changed files with 6068 additions and 27 deletions
--- a/commands/control_input_msgs/CMakeLists.txt
+++ b/commands/control_input_msgs/CMakeLists.txt
@ -0,0 +1,9 @@
+cmake_minimum_required(VERSION 3.8)
+project(control_input_msgs)
+
+find_package(rosidl_default_generators REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+        "msg/Inputs.msg"
+)
+
+ament_package()
--- a/commands/control_input_msgs/LICENSE
+++ b/commands/control_input_msgs/LICENSE
@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
--- a/commands/control_input_msgs/msg/Inputs.msg
+++ b/commands/control_input_msgs/msg/Inputs.msg
@ -0,0 +1,8 @@
+# control input message
+
+int32 command
+
+float32 lx
+float32 ly
+float32 rx
+float32 ry
--- a/commands/control_input_msgs/package.xml
+++ b/commands/control_input_msgs/package.xml
@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+    <name>control_input_msgs</name>
+    <version>0.0.0</version>
+    <description>TODO: Package description</description>
+    <maintainer email="biao876990970@hotmail.com">biao</maintainer>
+    <license>Apache-2.0</license>
+
+    <buildtool_depend>rosidl_default_generators</buildtool_depend>
+    <exec_depend>rosidl_default_runtime</exec_depend>
+    <member_of_group>rosidl_interface_packages</member_of_group>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>
--- a/commands/keyboard_input/CMakeLists.txt
+++ b/commands/keyboard_input/CMakeLists.txt
@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.8)
+project(keyboard_input)
+
+if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic)
+endif ()
+
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(control_input_msgs REQUIRED)
+
+add_executable(keyboard_input src/KeyboardInput.cpp)
+target_include_directories(keyboard_input
+        PUBLIC
+        "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+        "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+ament_target_dependencies(
+        keyboard_input PUBLIC
+        rclcpp
+        control_input_msgs
+)
+
+install(TARGETS
+        keyboard_input
+        DESTINATION lib/${PROJECT_NAME})
+
+ament_package()
--- a/commands/keyboard_input/LICENSE
+++ b/commands/keyboard_input/LICENSE
@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
--- a/commands/keyboard_input/include/keyboard_input/KeyboardInput.h
+++ b/commands/keyboard_input/include/keyboard_input/KeyboardInput.h
@ -0,0 +1,50 @@
+//
+// Created by biao on 24-9-11.
+//
+
+
+#ifndef KEYBOARDINPUT_H
+#define KEYBOARDINPUT_H
+#include <rclcpp/rclcpp.hpp>
+#include <control_input_msgs/msg/inputs.hpp>
+#include <termios.h>
+
+
+template <typename T1, typename T2>
+T1 max(const T1 a, const T2 b) {
+    return (a > b ? a : b);
+}
+
+template <typename T1, typename T2>
+T1 min(const T1 a, const T2 b) {
+    return (a < b ? a : b);
+}
+
+
+class Keyboardinput final : public rclcpp::Node {
+public:
+    Keyboardinput();
+
+    ~Keyboardinput() override {
+        tcsetattr(STDIN_FILENO, TCSANOW, &old_tio_);
+    }
+
+private:
+    void timer_callback();
+
+    void check_command(char key);
+    void check_value(char key);
+
+    static bool kbhit();
+
+    control_input_msgs::msg::Inputs inputs_;
+    rclcpp::Publisher<control_input_msgs::msg::Inputs>::SharedPtr publisher_;
+    rclcpp::TimerBase::SharedPtr timer_;
+
+    float sensitivity_left_ = 0.05;
+    float sensitivity_right_ = 0.05;
+    termios old_tio_{}, new_tio_{};
+};
+
+
+#endif //KEYBOARDINPUT_H
--- a/commands/keyboard_input/package.xml
+++ b/commands/keyboard_input/package.xml
@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+    <name>keyboard_input</name>
+    <version>0.0.0</version>
+    <description>TODO: Package description</description>
+    <maintainer email="biao876990970@hotmail.com">biao</maintainer>
+    <license>Apache-2.0</license>
+
+    <buildtool_depend>ament_cmake</buildtool_depend>
+
+    <depend>rclcpp</depend>
+    <depend>control_input_msgs</depend>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>
--- a/commands/keyboard_input/readme.md
+++ b/commands/keyboard_input/readme.md
@ -0,0 +1,15 @@
+# Keyboard Input
+
+This node will read the keyboard input and publish a control_input_msgs/Input message.
+
+## 1. Running Instructions
+### 1.1 Control Mode
+* Passive Mode: Keyboard 1
+* Fixed Stand: Keyboard 2
+    * Free Stand: Keyboard 3
+    * Trot: Keyboard 4
+    * SwingTest: Keyboard 9
+    * Balance: Keyboard 0
+### 1.2 Control Input
+* WASD IJKL: Move robot
+* Space: Reset Speed Input
--- a/commands/keyboard_input/src/KeyboardInput.cpp
+++ b/commands/keyboard_input/src/KeyboardInput.cpp
@ -0,0 +1,120 @@
+//
+// Created by biao on 24-9-11.
+//
+
+#include <keyboard_input/KeyboardInput.h>
+
+Keyboardinput::Keyboardinput() : Node("keyboard_input_node") {
+    publisher_ = create_publisher<control_input_msgs::msg::Inputs>("control_input", 10);
+    timer_ = create_wall_timer(std::chrono::microseconds(100), std::bind(&Keyboardinput::timer_callback, this));
+    inputs_ = control_input_msgs::msg::Inputs();
+
+    tcgetattr(STDIN_FILENO, &old_tio_);
+    new_tio_ = old_tio_;
+    new_tio_.c_lflag &= (~ICANON & ~ECHO);
+    tcsetattr(STDIN_FILENO, TCSANOW, &new_tio_);
+    RCLCPP_INFO(get_logger(), "Node initialized. Please input keys, press Ctrl+C to quit.");
+}
+
+void Keyboardinput::timer_callback() {
+    if (kbhit()) {
+        char key = getchar();
+        check_command(key);
+        if (inputs_.command == -1) check_value(key);
+        publisher_->publish(inputs_);
+    }
+}
+
+void Keyboardinput::check_command(const char key) {
+    switch (key) {
+        case '1':
+            inputs_.command = 1; // L2_B
+            break;
+        case '2':
+            inputs_.command = 2; // L2_A
+            break;
+        case '3':
+            inputs_.command = 3; // L2_X
+            break;
+        case '4':
+            inputs_.command = 4; // START
+            break;
+        case '0':
+            inputs_.command = 0; // L1_X
+            break;
+        case '9':
+            inputs_.command = 9; // L1_A
+            break;
+        case '8':
+            inputs_.command = 3; // L1_Y
+            break;
+        case ' ':
+            inputs_.lx = 0;
+            inputs_.ly = 0;
+            inputs_.rx = 0;
+            inputs_.ry = 0;
+            inputs_.command = -1;
+            break;
+        default:
+            inputs_.command = -1;
+            break;
+    }
+}
+
+void Keyboardinput::check_value(char key) {
+    switch (key) {
+        case 'w':
+        case 'W':
+            inputs_.ly = min<float>(inputs_.ly + sensitivity_left_, 1.0);
+            break;
+        case 's':
+        case 'S':
+            inputs_.ly = max<float>(inputs_.ly - sensitivity_left_, -1.0);
+            break;
+        case 'd':
+        case 'D':
+            inputs_.lx = min<float>(inputs_.lx + sensitivity_left_, 1.0);
+            break;
+        case 'a':
+        case 'A':
+            inputs_.lx = max<float>(inputs_.lx - sensitivity_left_, -1.0);
+            break;
+
+        case 'i':
+        case 'I':
+            inputs_.ry = min<float>(inputs_.ry + sensitivity_right_, 1.0);
+            break;
+        case 'k':
+        case 'K':
+            inputs_.ry = max<float>(inputs_.ry - sensitivity_right_, -1.0);
+            break;
+        case 'l':
+        case 'L':
+            inputs_.rx = min<float>(inputs_.rx + sensitivity_right_, 1.0);
+            break;
+        case 'j':
+        case 'J':
+            inputs_.rx = max<float>(inputs_.rx - sensitivity_right_, -1.0);
+            break;
+        default:
+            break;
+    }
+}
+
+bool Keyboardinput::kbhit() {
+    timeval tv = {0L, 0L};
+    fd_set fds;
+    FD_ZERO(&fds);
+    FD_SET(STDIN_FILENO, &fds);
+    return select(STDIN_FILENO + 1, &fds, NULL, NULL, &tv);
+}
+
+
+int main(int argc, char *argv[])
+{
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<Keyboardinput>();
+    spin(node);
+    rclcpp::shutdown();
+    return 0;
+}
--- a/commands/readme.md
+++ b/commands/readme.md
@ -0,0 +1,3 @@
+# Control Command Inputs
+
+This folder contains the ros2 node for control input, like keyboard or gamepad.
--- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h
@ -2,14 +2,13 @@
 // Created by biao on 24-9-10.
 //

-#include "FSMState.h"
-
 #ifndef STATEFIXEDSTAND_H
 #define STATEFIXEDSTAND_H

-
+#include "FSMState.h"

 class StateFixedStand : public FSMState {
+public:
    explicit StateFixedStand(CtrlComponent ctrlComp);

    void enter() override;
@ -22,5 +21,4 @@ class StateFixedStand : public FSMState {
 };


-
 #endif //STATEFIXEDSTAND_H
--- a/descriptions/go1_description/CMakeLists.txt
+++ b/descriptions/go1_description/CMakeLists.txt
@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.8)
+project(go1_description)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+        DIRECTORY meshes xacro launch config
+        DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()
--- a/descriptions/go1_description/config/robot_control.yaml
+++ b/descriptions/go1_description/config/robot_control.yaml
@ -0,0 +1,9 @@
+# Controller Manager configuration
+controller_manager:
+  ros__parameters:
+    update_rate: 200  # Hz
+    use_sim_time: true  # If running in simulation
+
+    # Define the available controllers
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
--- a/descriptions/go1_description/config/visualize_urdf.rviz
+++ b/descriptions/go1_description/config/visualize_urdf.rviz
@ -0,0 +1,255 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 138
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+      Splitter Ratio: 0.5
+    Tree Height: 832
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        FL_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        Link Tree Style: Links in Alphabetic Order
+        RL_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        trunk:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: base
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 1.469738483428955
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.0716833621263504
+        Y: -0.002330765128135681
+        Z: -0.15908166766166687
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5053979158401489
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.8203961253166199
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: true
+  Height: 1194
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f40000043cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000700000043c0000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015d0000043cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000700000043c0000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006180000005efc0100000002fb0000000800540069006d00650100000000000006180000047a00fffffffb0000000800540069006d0065010000000000000450000000000000000000000618000003a000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1560
+  X: 1327
+  Y: 195
--- a/descriptions/go1_description/launch/hardware.launch.py
+++ b/descriptions/go1_description/launch/hardware.launch.py
@ -0,0 +1,89 @@
+import os
+
+import xacro
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+package_description = "go1_description"
+
+
+def process_xacro(context):
+    robot_type_value = context.launch_configurations['robot_type']
+    pkg_path = os.path.join(get_package_share_directory(package_description))
+    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
+    robot_description_config = xacro.process_file(xacro_file, mappings={'robot_type': robot_type_value})
+    return (robot_description_config.toxml(), robot_type_value)
+
+
+def launch_setup(context, *args, **kwargs):
+    (robot_description, robot_type) = process_xacro(context)
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare(package_description),
+            "config",
+            "robot_control.yaml",
+        ]
+    )
+    return [
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            name='robot_state_publisher',
+            parameters=[
+                {
+                    'publish_frequency': 100.0,
+                    'use_tf_static': True,
+                    'robot_description': robot_description,
+                    'ignore_timestamp': True
+                }
+            ],
+        ),
+        Node(
+            package="controller_manager",
+            executable="ros2_control_node",
+            parameters=[robot_controllers],
+            remappings=[
+                ("~/robot_description", "/robot_description"),
+            ],
+            output="both",
+        ),
+        Node(
+            package="controller_manager",
+            executable="spawner",
+            arguments=["joint_state_broadcaster",
+                       "--controller-manager", "/controller_manager"],
+        ),
+        # Node(
+        #     package="controller_manager",
+        #     executable="spawner",
+        #     arguments=["imu_sensor_broadcaster",
+        #                "--controller-manager", "/controller_manager"],
+        # ),
+    ]
+
+
+def generate_launch_description():
+    robot_type_arg = DeclareLaunchArgument(
+        'robot_type',
+        default_value='go1',
+        description='Type of the robot'
+    )
+
+    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
+
+    return LaunchDescription([
+        robot_type_arg,
+        OpaqueFunction(function=launch_setup),
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz_ocs2',
+            output='screen',
+            arguments=["-d", rviz_config_file]
+        )
+    ])
--- a/descriptions/go1_description/launch/visualize.launch.py
+++ b/descriptions/go1_description/launch/visualize.launch.py
@ -0,0 +1,65 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch_ros.actions import Node
+
+import xacro
+
+package_description = "go1_description"
+
+def process_xacro(context):
+    robot_type_value = context.launch_configurations['robot_type']
+    pkg_path = os.path.join(get_package_share_directory(package_description))
+    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
+    robot_description_config = xacro.process_file(xacro_file, mappings={'robot_type': robot_type_value})
+    return robot_description_config.toxml()
+
+def launch_setup(context, *args, **kwargs):
+    robot_description = process_xacro(context)
+    return [
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            name='robot_state_publisher',
+            output='screen',
+            parameters=[
+                {
+                    'publish_frequency': 100.0,
+                    'use_tf_static': True,
+                    'robot_description': robot_description
+                }
+            ],
+        ),
+        Node(
+            package='joint_state_publisher_gui',
+            executable='joint_state_publisher_gui',
+            name='joint_state_publisher',
+            output='screen',
+        )
+    ]
+
+
+def generate_launch_description():
+
+    robot_type_arg = DeclareLaunchArgument(
+        'robot_type',
+        default_value='go1',
+        description='Type of the robot'
+    )
+    
+    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
+
+    return LaunchDescription([
+        robot_type_arg,
+        OpaqueFunction(function=launch_setup),
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz_ocs2',
+            output='screen',
+            arguments=["-d", rviz_config_file]
+        )
+    ])
--- a/descriptions/go1_description/meshes/calf.dae
+++ b/descriptions/go1_description/meshes/calf.dae
--- a/descriptions/go1_description/meshes/depthCamera.dae
+++ b/descriptions/go1_description/meshes/depthCamera.dae
--- a/descriptions/go1_description/meshes/hip.dae
+++ b/descriptions/go1_description/meshes/hip.dae
--- a/descriptions/go1_description/meshes/thigh.dae
+++ b/descriptions/go1_description/meshes/thigh.dae
--- a/descriptions/go1_description/meshes/thigh_mirror.dae
+++ b/descriptions/go1_description/meshes/thigh_mirror.dae
--- a/descriptions/go1_description/meshes/trunk.dae
+++ b/descriptions/go1_description/meshes/trunk.dae
--- a/descriptions/go1_description/meshes/ultraSound.dae
+++ b/descriptions/go1_description/meshes/ultraSound.dae
--- a/descriptions/go1_description/package.xml
+++ b/descriptions/go1_description/package.xml
@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<package format="2">
+    <name>go1_description</name>
+    <version>0.0.0</version>
+    <description>The go1_description package</description>
+
+    <maintainer email="laikago@unitree.cc">unitree</maintainer>
+    <license>TODO</license>
+
+    <exec_depend>joint_state_publisher</exec_depend>
+    <exec_depend>robot_state_publisher</exec_depend>
+    <exec_depend>imu_sensor_broadcaster</exec_depend>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>
--- a/descriptions/go1_description/readme.md
+++ b/descriptions/go1_description/readme.md
@ -0,0 +1,21 @@
+# Unitree Go1 Description
+This repository contains the urdf model of go1.
+
+## Build
+```bash
+cd ~/ros2_ws
+colcon build --packages-up-to go1_description
+```
+
+## Visualize the robot
+To visualize and check the configuration of the robot in rviz, simply launch:
+```bash
+source ~/ros2_ws/install/setup.bash
+ros2 launch go1_description visualize.launch.py
+```
+
+## Launch Hardware Interface
+```bash
+source ~/ros2_ws/install/setup.bash
+ros2 launch go1_description hardware.launch.py
+```
--- a/descriptions/go1_description/xacro/const.xacro
+++ b/descriptions/go1_description/xacro/const.xacro
@ -0,0 +1,159 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <!-- Constants for robot dimensions -->
+    <xacro:property name="PI" value="3.1415926535897931"/>
+    <xacro:property name="stick_mass" value="0.00001"/>
+
+    <!-- simplified collision value -->
+    <xacro:property name="trunk_width" value="0.0935"/> 
+    <xacro:property name="trunk_length" value="0.3762"/>
+    <xacro:property name="trunk_height" value="0.114"/>
+    <xacro:property name="hip_radius" value="0.046"/>
+    <xacro:property name="hip_length" value="0.04"/>
+    <xacro:property name="thigh_shoulder_radius" value="0.041"/>
+    <xacro:property name="thigh_shoulder_length" value="0.032"/>
+    <xacro:property name="thigh_width" value="0.0245"/>
+    <xacro:property name="thigh_height" value="0.034"/>
+    <xacro:property name="calf_width" value="0.016"/>
+    <xacro:property name="calf_height" value="0.016"/>
+    <xacro:property name="foot_radius" value="0.02"/>
+    <xacro:property name="stick_radius" value="0.01"/>
+    <xacro:property name="stick_length" value="0.2"/>
+
+    <!-- kinematic value -->
+    <xacro:property name="thigh_offset" value="0.08"/>
+    <xacro:property name="thigh_length" value="0.213"/>
+    <xacro:property name="calf_length" value="0.213"/>
+
+    <!-- leg offset from trunk center value -->
+    <xacro:property name="leg_offset_x" value="0.1881"/>
+    <xacro:property name="leg_offset_y" value="0.04675"/>
+    <!-- <xacro:property name="trunk_offset_z" value="0.01675"/> -->
+    <xacro:property name="hip_offset" value="0.08"/>
+
+    <!-- offset of link and rotor locations (left front) -->
+    <xacro:property name="hip_offset_x" value="0.1881"/>
+    <xacro:property name="hip_offset_y" value="0.04675"/>
+    <xacro:property name="hip_offset_z" value="0.0"/>
+    <xacro:property name="hip_rotor_offset_x" value="0.11215"/>
+    <xacro:property name="hip_rotor_offset_y" value="0.04675"/>
+    <xacro:property name="hip_rotor_offset_z" value="0.0"/>
+
+    <xacro:property name="thigh_offset_x" value="0"/>
+    <xacro:property name="thigh_offset_y" value="0.08"/>
+    <xacro:property name="thigh_offset_z" value="0.0"/>
+    <xacro:property name="thigh_rotor_offset_x" value="0.0"/>
+    <xacro:property name="thigh_rotor_offset_y" value="-0.00015"/>
+    <xacro:property name="thigh_rotor_offset_z" value="0.0"/>
+
+    <xacro:property name="calf_offset_x" value="0.0"/>
+    <xacro:property name="calf_offset_y" value="0.0"/>
+    <xacro:property name="calf_offset_z" value="-0.213"/>
+    <xacro:property name="calf_rotor_offset_x" value="0.0"/>
+    <xacro:property name="calf_rotor_offset_y" value="-0.03235"/>
+    <xacro:property name="calf_rotor_offset_z" value="0.0"/>
+
+    <!-- joint limits -->
+    <xacro:property name="damping" value="0.0"/>
+    <xacro:property name="friction" value="0.0"/>
+    <xacro:property name="hip_position_max" value="0.863"/>
+    <xacro:property name="hip_position_min" value="-0.863"/>
+    <xacro:property name="hip_velocity_max" value="30.1"/>
+    <xacro:property name="hip_torque_max" value="23.7"/>
+    <xacro:property name="thigh_position_max" value="4.501"/>
+    <xacro:property name="thigh_position_min" value="-0.686"/>
+    <xacro:property name="thigh_velocity_max" value="30.1"/>
+    <xacro:property name="thigh_torque_max" value="23.7"/>
+    <xacro:property name="calf_position_max" value="-0.888"/>
+    <xacro:property name="calf_position_min" value="-2.818"/>
+    <xacro:property name="calf_velocity_max" value="20.06"/>
+    <xacro:property name="calf_torque_max" value="35.55"/>
+
+    <!-- dynamics inertial value        total 12.84kg -->
+    <!-- trunk -->
+    <xacro:property name="trunk_mass" value="5.204"/>
+    <xacro:property name="trunk_com_x" value="0.0223"/>
+    <xacro:property name="trunk_com_y" value="0.002"/>
+    <xacro:property name="trunk_com_z" value="-0.0005"/>
+    <xacro:property name="trunk_ixx" value="0.0168128557"/>
+    <xacro:property name="trunk_ixy" value="-0.0002296769"/>
+    <xacro:property name="trunk_ixz" value="-0.0002945293"/>
+    <xacro:property name="trunk_iyy" value="0.063009565"/>
+    <xacro:property name="trunk_iyz" value="-0.0000418731"/>
+    <xacro:property name="trunk_izz" value="0.0716547275"/>
+
+    <!-- hip (left front) -->
+    <xacro:property name="hip_mass" value="0.591"/>
+    <xacro:property name="hip_com_x" value="-0.005657"/>
+    <xacro:property name="hip_com_y" value="-0.008752"/>
+    <xacro:property name="hip_com_z" value="-0.000102"/>
+    <xacro:property name="hip_ixx" value="0.000334008405"/>
+    <xacro:property name="hip_ixy" value="-0.000010826066"/>
+    <xacro:property name="hip_ixz" value="0.000001290732"/>
+    <xacro:property name="hip_iyy" value="0.000619101213"/>
+    <xacro:property name="hip_iyz" value="0.000001643194"/>
+    <xacro:property name="hip_izz" value="0.00040057614"/>
+
+    <xacro:property name="hip_rotor_mass" value="0.089"/>
+    <xacro:property name="hip_rotor_com_x" value="0.0"/>
+    <xacro:property name="hip_rotor_com_y" value="0.0"/>
+    <xacro:property name="hip_rotor_com_z" value="0.0"/>
+    <xacro:property name="hip_rotor_ixx" value="0.000111842"/>
+    <xacro:property name="hip_rotor_ixy" value="0.0"/>
+    <xacro:property name="hip_rotor_ixz" value="0.0"/>
+    <xacro:property name="hip_rotor_iyy" value="0.000059647"/>
+    <xacro:property name="hip_rotor_iyz" value="0.0"/>
+    <xacro:property name="hip_rotor_izz" value="0.000059647"/>
+
+    <!-- thigh -->
+    <xacro:property name="thigh_mass" value="0.92"/>
+    <xacro:property name="thigh_com_x" value="-0.003342"/>
+    <xacro:property name="thigh_com_y" value="-0.018054"/>
+    <xacro:property name="thigh_com_z" value="-0.033451"/>
+    <xacro:property name="thigh_ixx" value="0.004431760472"/>
+    <xacro:property name="thigh_ixy" value="0.000057496807"/>
+    <xacro:property name="thigh_ixz" value="-0.000218457134"/>
+    <xacro:property name="thigh_iyy" value="0.004485671726"/>
+    <xacro:property name="thigh_iyz" value="0.000572001265"/>
+    <xacro:property name="thigh_izz" value="0.000740309489"/>
+
+    <xacro:property name="thigh_rotor_mass" value="0.089"/>
+    <xacro:property name="thigh_rotor_com_x" value="0.0"/>
+    <xacro:property name="thigh_rotor_com_y" value="0.0"/>
+    <xacro:property name="thigh_rotor_com_z" value="0.0"/>
+    <xacro:property name="thigh_rotor_ixx" value="0.000059647"/>
+    <xacro:property name="thigh_rotor_ixy" value="0.0"/>
+    <xacro:property name="thigh_rotor_ixz" value="0.0"/>
+    <xacro:property name="thigh_rotor_iyy" value="0.000111842"/>
+    <xacro:property name="thigh_rotor_iyz" value="0.0"/>
+    <xacro:property name="thigh_rotor_izz" value="0.000059647"/>
+
+    <!-- calf -->
+    <xacro:property name="calf_mass" value="0.135862"/>
+    <xacro:property name="calf_com_x" value="0.006197"/>
+    <xacro:property name="calf_com_y" value="0.001408"/>
+    <xacro:property name="calf_com_z" value="-0.116695"/>
+    <xacro:property name="calf_ixx" value="0.001088793059"/>
+    <xacro:property name="calf_ixy" value="-0.000000255679"/>
+    <xacro:property name="calf_ixz" value="0.000007117814"/>
+    <xacro:property name="calf_iyy" value="0.001100428748"/>
+    <xacro:property name="calf_iyz" value="0.000002077264"/>
+    <xacro:property name="calf_izz" value="0.000024787446"/>
+
+    <xacro:property name="calf_rotor_mass" value="0.089"/>
+    <xacro:property name="calf_rotor_com_x" value="0.0"/>
+    <xacro:property name="calf_rotor_com_y" value="0.0"/>
+    <xacro:property name="calf_rotor_com_z" value="0.0"/>
+    <xacro:property name="calf_rotor_ixx" value="0.000059647"/>
+    <xacro:property name="calf_rotor_ixy" value="0.0"/>
+    <xacro:property name="calf_rotor_ixz" value="0.0"/>
+    <xacro:property name="calf_rotor_iyy" value="0.000111842"/>
+    <xacro:property name="calf_rotor_iyz" value="0.0"/>
+    <xacro:property name="calf_rotor_izz" value="0.000059647"/>
+
+    <!-- foot -->
+    <xacro:property name="foot_mass" value="0.06"/>
+
+</robot>
--- a/descriptions/go1_description/xacro/depthCamera.xacro
+++ b/descriptions/go1_description/xacro/depthCamera.xacro
@ -0,0 +1,87 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:macro name="depthCamera" params="camID name *origin">
+        <joint name="camera_joint_${name}" type="fixed">
+            <xacro:insert_block name="origin"/>
+            <parent link="trunk"/>
+            <child link="camera_${name}"/>
+        </joint>
+
+        <link name="camera_${name}">
+            <collision>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <box size=".001 .001 .001"/>
+            </geometry>
+            </collision>
+
+            <visual>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <mesh filename="file://$(find go1_description)/meshes/depthCamera.dae" scale="1 1 1"/>
+            </geometry>
+            </visual>
+
+            <inertial>
+            <mass value="1e-5" />
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
+            </inertial>
+        </link>
+
+        <joint name="camera_optical_joint_${name}" type="fixed">
+            <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
+            <parent link="camera_${name}"/>
+            <child link="camera_optical_${name}"/>
+        </joint>
+
+        <link name="camera_optical_${name}">
+        </link>
+
+        <gazebo reference="camera_${name}">
+            <!-- <material>Gazebo/Black</material> -->
+            <sensor name="camera_${name}_camera" type="depth">
+                <update_rate>16</update_rate>
+                <camera>
+                    <horizontal_fov>2.094</horizontal_fov>
+                    <image>
+                        <width>928</width>
+                        <height>800</height>
+                        <format>R8G8B8</format>
+                    </image>
+                    <clip>
+                        <near>0.1</near>
+                        <far>5</far>
+                    </clip>
+                </camera>
+                <plugin name="camera_${name}_controller" filename="libgazebo_ros_openni_kinect.so">
+                    <baseline>0.025</baseline>
+                    <alwaysOn>true</alwaysOn>
+                    <updateRate>0.0</updateRate>
+                    <cameraName>camera_${name}_ir</cameraName>
+                    <imageTopicName>/camera_${name}/color/image_raw</imageTopicName>
+                    <cameraInfoTopicName>/camera_${name}/color/camera_info</cameraInfoTopicName>
+                    <depthImageTopicName>/camera_${name}/depth/image_raw</depthImageTopicName>
+                    <depthImageInfoTopicName>/camera_${name}/depth/camera_info</depthImageInfoTopicName>
+                    <!-- <pointCloudTopicName>/camera_${name}/depth/points</pointCloudTopicName> -->
+                    <pointCloudTopicName>/cam${camID}/point_cloud_${name}</pointCloudTopicName>
+                    <frameName>camera_optical_${name}</frameName>
+                    <pointCloudCutoff>0.1</pointCloudCutoff>
+                    <pointCloudCutoffMax>1.5</pointCloudCutoffMax>
+                    <distortionK1>0.0</distortionK1>
+                    <distortionK2>0.0</distortionK2>
+                    <distortionK3>0.0</distortionK3>
+                    <distortionT1>0.0</distortionT1>
+                    <distortionT2>0.0</distortionT2>
+                    <CxPrime>0</CxPrime>
+                    <Cx>0.0045</Cx>
+                    <Cy>0.0039</Cy>
+                    <focalLength>0</focalLength>
+                    <hackBaseline>0</hackBaseline>
+                </plugin>
+            </sensor>
+        </gazebo>
+
+    </xacro:macro>
+</robot>
--- a/descriptions/go1_description/xacro/gazebo.xacro
+++ b/descriptions/go1_description/xacro/gazebo.xacro
@ -0,0 +1,385 @@
+<?xml version="1.0"?>
+<robot>
+    <!-- ros_control plugin -->
+    <gazebo>
+        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+            <robotNamespace>/go1_gazebo</robotNamespace>
+            <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+        </plugin>
+    </gazebo>
+
+    <!-- Show the trajectory of trunk center. -->
+    <gazebo>
+        <plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
+            <frequency>10</frequency>
+            <plot>
+                <link>base</link>
+                <pose>0 0 0 0 0 0</pose>
+                <material>Gazebo/Yellow</material>
+            </plot>
+        </plugin>
+    </gazebo>
+
+    <!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
+    <!-- <gazebo>
+        <plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
+            <frequency>100</frequency>
+            <plot>
+                <link>FL_foot</link>
+                <pose>0 0 0 0 0 0</pose>
+                <material>Gazebo/Green</material>
+            </plot>
+        </plugin>
+    </gazebo> -->
+
+    <gazebo>
+        <plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
+            <bodyName>trunk</bodyName>
+            <topicName>/apply_force/trunk</topicName>
+        </plugin>
+    </gazebo>
+
+    <gazebo reference="imu_link">
+        <gravity>true</gravity>
+        <sensor name="imu_sensor" type="imu">
+            <always_on>true</always_on>
+            <update_rate>1000</update_rate>
+            <visualize>true</visualize>
+            <topic>__default_topic__</topic>
+            <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
+                <topicName>trunk_imu</topicName>
+                <bodyName>imu_link</bodyName>
+                <updateRateHZ>1000.0</updateRateHZ>
+                <gaussianNoise>0.0</gaussianNoise>
+                <xyzOffset>0 0 0</xyzOffset>
+                <rpyOffset>0 0 0</rpyOffset>
+                <frameName>imu_link</frameName>
+            </plugin>
+            <pose>0 0 0 0 0 0</pose>
+        </sensor>
+    </gazebo>
+
+    <!-- Depth camera -->
+    <!-- <gazebo>
+        <plugin name="camera_plugin" filename="libgazebo_ros_openni_kinect.so">
+            <baseline>0.025</baseline>
+            <always_on>true</always_on>
+            <updateRate>0.0</updateRate>
+            <cameraName>unitree_camera_left</cameraName>
+            <frameName>depthCamera_link_left</frameName>
+            <imageTopicName>rgb/imageRaw_left</imageTopicName>
+            <depthImageTopicName>depth/imageRaw_left</depthImageTopicName>
+            <pointCloudTopicName>depth/points_left</pointCloudTopicName>
+            <cameraInfoTopicName>rgb/cameraInfo_left</cameraInfoTopicName>              
+            <depthImageCameraInfoTopicName>depth/cameraInfo_left</depthImageCameraInfoTopicName>            
+            <pointCloudCutoff>0.1</pointCloudCutoff>                
+            <pointCloudCutoffMax>1.5</pointCloudCutoffMax>                
+            <distortionK1>0.0</distortionK1>
+            <distortionK2>0.0</distortionK2>
+            <distortionK3>0.0</distortionK3>
+            <distortionT1>0.0</distortionT1>
+            <distortionT2>0.0</distortionT2>
+            <CxPrime>0.0</CxPrime>
+            <Cx>0.0045</Cx>
+            <Cy>0.0039</Cy>
+            <focalLength>0.004</focalLength>
+            <hackBaseline>0.0</hackBaseline>
+        </plugin>
+    </gazebo> -->
+
+    <!-- <gazebo reference="depthCamera_link_left">
+        <sensor name="unitree_camera_left" type="depth_camera">
+            <update_rate>16</update_rate>
+            <always_on>true</always_on>
+            <visualize>true</visualize>
+            <camera>
+                <horizontal_fov>2.094</horizontal_fov>
+                <image>
+                    <width>928</width>
+                    <height>800</height>
+                    <format>R8G8B8</format>
+                </image>
+                <depth_camera></depth_camera>
+                <clip>
+                    <near>0.1</near>
+                    <far>100</far>
+                </clip>
+            </camera>
+            <plugin name="camera_plugin" filename="libgazebo_ros_openni_kinect.so">
+                <baseline>0.025</baseline>
+                <always_on>true</always_on>
+                <updateRate>0.0</updateRate>
+                <cameraName>unitree_camera_left</cameraName>
+                <frameName>depthCamera_link_left</frameName>
+                <imageTopicName>rgb/imageRaw_left</imageTopicName>
+                <depthImageTopicName>depth/imageRaw_left</depthImageTopicName>
+                <pointCloudTopicName>depth/points_left</pointCloudTopicName>
+                <cameraInfoTopicName>rgb/cameraInfo_left</cameraInfoTopicName>              
+                <depthImageCameraInfoTopicName>depth/cameraInfo_left</depthImageCameraInfoTopicName>            
+                <pointCloudCutoff>0.1</pointCloudCutoff>                
+                <pointCloudCutoffMax>1.5</pointCloudCutoffMax>                
+                <distortionK1>0.0</distortionK1>
+                <distortionK2>0.0</distortionK2>
+                <distortionK3>0.0</distortionK3>
+                <distortionT1>0.0</distortionT1>
+                <distortionT2>0.0</distortionT2>
+                <CxPrime>0.0</CxPrime>
+                <Cx>0.0045</Cx>
+                <Cy>0.0039</Cy>
+                <focalLength>0.004</focalLength>
+                <hackBaseline>0.0</hackBaseline>
+            </plugin>
+        </sensor>
+    </gazebo> -->
+
+  <gazebo reference="depthCamera_link_left">
+    <sensor name="camera_link_camera" type="depth">
+      <update_rate>20</update_rate>
+      <camera>
+        <horizontal_fov>1.047198</horizontal_fov>
+        <image>
+          <width>640</width>
+          <height>480</height>
+          <format>R8G8B8</format>
+        </image>
+        <clip>
+          <near>0.05</near>
+          <far>3</far>
+        </clip>
+      </camera>
+      <plugin name="camera_link_controller" filename="libgazebo_ros_openni_kinect.so">
+        <baseline>0.2</baseline>
+        <alwaysOn>true</alwaysOn>
+        <updateRate>1.0</updateRate>
+        <cameraName>camera_link_ir</cameraName>
+        <imageTopicName>/camera_link/color/image_raw</imageTopicName>
+        <cameraInfoTopicName>/camera_link/color/camera_info</cameraInfoTopicName>
+        <depthImageTopicName>/camera_link/depth/image_raw</depthImageTopicName>
+        <depthImageInfoTopicName>/camera_link/depth/camera_info</depthImageInfoTopicName>
+        <pointCloudTopicName>/camera_link/depth/points</pointCloudTopicName>
+        <frameName>depthCamera_link_left</frameName>
+        <pointCloudCutoff>0.5</pointCloudCutoff>
+        <pointCloudCutoffMax>3.0</pointCloudCutoffMax>
+        <distortionK1>0.00000001</distortionK1>
+        <distortionK2>0.00000001</distortionK2>
+        <distortionK3>0.00000001</distortionK3>
+        <distortionT1>0.00000001</distortionT1>
+        <distortionT2>0.00000001</distortionT2>
+        <CxPrime>0</CxPrime>
+        <Cx>0</Cx>
+        <Cy>0</Cy>
+        <focalLength>0</focalLength>
+        <hackBaseline>0</hackBaseline>
+      </plugin>
+    </sensor>
+  </gazebo>
+
+
+
+
+
+    <!-- Foot contacts. -->
+    <gazebo reference="FR_calf">
+        <sensor name="FR_foot_contact" type="contact">
+        <update_rate>100</update_rate>
+        <plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
+        <contact>
+            <collision>FR_calf_fixed_joint_lump__FR_foot_collision_1</collision>
+        </contact>
+        </sensor>
+    </gazebo>
+    <gazebo reference="FL_calf">
+        <sensor name="FL_foot_contact" type="contact">
+        <update_rate>100</update_rate>
+        <plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
+        <contact>
+            <collision>FL_calf_fixed_joint_lump__FL_foot_collision_1</collision>
+        </contact>
+        </sensor>
+    </gazebo>
+    <gazebo reference="RR_calf">
+        <sensor name="RR_foot_contact" type="contact">
+        <update_rate>100</update_rate>
+        <plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
+        <contact>
+            <collision>RR_calf_fixed_joint_lump__RR_foot_collision_1</collision>
+        </contact>
+        </sensor>
+    </gazebo>
+    <gazebo reference="RL_calf">
+        <sensor name="RL_foot_contact" type="contact">
+        <update_rate>100</update_rate>
+        <plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
+        <contact>
+            <collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
+        </contact>
+        </sensor>
+    </gazebo>
+
+    <!-- Visualization of Foot contacts. -->
+    <gazebo reference="FR_foot">
+        <visual>
+            <plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
+                <topicName>FR_foot_contact</topicName>
+            </plugin>
+        </visual>
+    </gazebo>
+    <gazebo reference="FL_foot">
+        <visual>
+            <plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
+                <topicName>FL_foot_contact</topicName>
+            </plugin>
+        </visual>
+    </gazebo>
+    <gazebo reference="RR_foot">
+        <visual>
+            <plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
+                <topicName>RR_foot_contact</topicName>
+            </plugin>
+        </visual>
+    </gazebo>
+    <gazebo reference="RL_foot">
+        <visual>
+            <plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
+                <topicName>RL_foot_contact</topicName>
+            </plugin>
+        </visual>
+    </gazebo>
+
+    <gazebo reference="base">
+        <material>Gazebo/Green</material>
+        <turnGravityOff>false</turnGravityOff>
+    </gazebo>
+
+    <gazebo reference="trunk">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+
+    <gazebo reference="stick_link">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/White</material>
+    </gazebo>
+
+    <gazebo reference="imu_link">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/Red</material>
+    </gazebo>
+
+    <!-- FL leg -->
+    <gazebo reference="FL_hip">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/DarkGrey</material>
+    </gazebo>
+    <gazebo reference="FL_thigh">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+        <material>Gazebo/DarkGrey</material>
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+    <gazebo reference="FL_calf">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+    </gazebo>
+    <gazebo reference="FL_foot">
+        <mu1>0.6</mu1>
+        <mu2>0.6</mu2>
+        <self_collide>1</self_collide>
+        <material>Gazebo/DarkGrey</material>
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+
+    <!-- FR leg -->
+    <gazebo reference="FR_hip">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/DarkGrey</material>
+    </gazebo>
+    <gazebo reference="FR_thigh">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+        <material>Gazebo/DarkGrey</material>
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+    <gazebo reference="FR_calf">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+    </gazebo>
+    <gazebo reference="FR_foot">
+        <mu1>0.6</mu1>
+        <mu2>0.6</mu2>
+        <self_collide>1</self_collide>
+        <material>Gazebo/DarkGrey</material>
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+
+    <!-- RL leg -->
+    <gazebo reference="RL_hip">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/DarkGrey</material>
+    </gazebo>
+    <gazebo reference="RL_thigh">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+        <material>Gazebo/DarkGrey</material>
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+    <gazebo reference="RL_calf">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+    </gazebo>
+    <gazebo reference="RL_foot">
+        <mu1>0.6</mu1>
+        <mu2>0.6</mu2>
+        <self_collide>1</self_collide>
+        <material>Gazebo/DarkGrey</material>
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+
+    <!-- RR leg -->
+    <gazebo reference="RR_hip">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/DarkGrey</material>
+    </gazebo>
+    <gazebo reference="RR_thigh">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+        <material>Gazebo/DarkGrey</material>
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+    <gazebo reference="RR_calf">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+    </gazebo>
+    <gazebo reference="RR_foot">
+        <mu1>0.6</mu1>
+        <mu2>0.6</mu2>
+        <self_collide>1</self_collide>
+        <material>Gazebo/DarkGrey</material>
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+
+</robot>
--- a/descriptions/go1_description/xacro/leg.xacro
+++ b/descriptions/go1_description/xacro/leg.xacro
@ -0,0 +1,272 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:include filename="$(find go1_description)/xacro/transmission.xacro"/>
+
+    <xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae">
+
+        <joint name="${name}_hip_joint" type="revolute">
+            <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
+                <origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
+                <origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
+                <origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
+                <origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
+            </xacro:if>
+            <parent link="trunk"/>
+            <child link="${name}_hip"/>
+            <axis xyz="1 0 0"/>
+            <dynamics damping="${damping}" friction="${friction}"/>
+            <xacro:if value="${(mirror_dae == True)}">
+                <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_position_min}" upper="${hip_position_max}"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == False)}">
+                <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_position_max}" upper="${-hip_position_min}"/>
+            </xacro:if>            
+        </joint>
+
+        <joint name="${name}_hip_rotor_joint" type="fixed">
+            <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
+                <origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
+                <origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
+                <origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
+                <origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
+            </xacro:if>
+            <parent link="trunk"/>
+            <child link="${name}_hip_rotor"/>         
+        </joint>
+
+        <link name="${name}_hip">
+            <visual>
+                <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
+                    <origin rpy="0 0 0" xyz="0 0 0"/>
+                </xacro:if>
+                <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
+                    <origin rpy="${PI} 0 0" xyz="0 0 0"/>
+                </xacro:if>
+                <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
+                    <origin rpy="0 ${PI} 0" xyz="0 0 0"/>
+                </xacro:if>
+                <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
+                    <origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
+                </xacro:if>
+                <geometry>
+                    <mesh filename="file://$(find go1_description)/meshes/hip.dae" scale="1 1 1"/>
+                </geometry>
+                <!-- <material name="orange"/> -->
+            </visual>
+            <collision>
+                <origin rpy="${PI/2.0} 0 0" xyz="0 ${hip_offset*mirror} 0"/>
+                <geometry>
+                    <cylinder length="${hip_length}" radius="${hip_radius}"/>
+                </geometry>
+            </collision>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
+                <mass value="${hip_mass}"/>
+                <inertia
+                    ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
+                    iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
+                    izz="${hip_izz}"/>       
+            </inertial>
+        </link>
+
+        <link name="${name}_hip_rotor">
+            <visual>
+                <origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
+                <geometry>
+                    <cylinder length="0.02" radius="0.035"/>
+                </geometry>
+                <material name="green"/>
+            </visual>
+            <!-- <collision>
+                <origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
+                <geometry>
+                    <cylinder length="0.02" radius="0.035"/>
+                </geometry>
+            </collision> -->
+            <inertial>
+                <origin rpy="0 0 0" xyz="${hip_rotor_com_x} ${hip_rotor_com_y} ${hip_rotor_com_z}"/>
+                <mass value="${hip_rotor_mass}"/>
+                <inertia
+                    ixx="${hip_rotor_ixx}" ixy="${hip_rotor_ixy}" ixz="${hip_rotor_ixz}"
+                    iyy="${hip_rotor_iyy}" iyz="${hip_rotor_iyz}"
+                    izz="${hip_rotor_izz}"/>              
+            </inertial>
+        </link>
+
+        <joint name="${name}_thigh_joint" type="revolute">
+            <origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
+            <parent link="${name}_hip"/>
+            <child link="${name}_thigh"/>
+            <axis xyz="0 1 0"/>
+            <dynamics damping="${damping}" friction="${friction}"/>
+            <limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_position_min}" upper="${thigh_position_max}"/>
+        </joint>
+
+        <joint name="${name}_thigh_rotor_joint" type="fixed">
+            <origin rpy="0 0 0" xyz="${thigh_rotor_offset_x} ${thigh_rotor_offset_y*mirror} 0"/>
+            <parent link="${name}_hip"/>
+            <child link="${name}_thigh_rotor"/>         
+        </joint>
+
+        <link name="${name}_thigh">
+            <visual>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <xacro:if value="${mirror_dae == True}">
+                        <mesh filename="file://$(find go1_description)/meshes/thigh.dae" scale="1 1 1"/>
+                    </xacro:if>
+                    <xacro:if value="${mirror_dae == False}">
+                        <mesh filename="file://$(find go1_description)/meshes/thigh_mirror.dae" scale="1 1 1"/>
+                    </xacro:if>
+                </geometry>
+                <!-- <material name="orange"/> -->
+            </visual>
+            <collision>
+                <origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/>
+                <geometry>
+                    <box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
+                </geometry>      
+            </collision>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
+                <mass value="${thigh_mass}"/>
+                <inertia
+                    ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
+                    iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
+                    izz="${thigh_izz}"/>       
+            </inertial>
+        </link>
+
+        <link name="${name}_thigh_rotor">
+            <visual>
+                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <cylinder length="0.02" radius="0.035"/>
+                </geometry>
+                <material name="green"/>
+            </visual>
+            <!-- <collision>
+                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <cylinder length="0.02" radius="0.035"/>
+                </geometry>
+            </collision> -->
+            <inertial>
+                <origin rpy="0 0 0" xyz="${thigh_rotor_com_x} ${thigh_rotor_com_y} ${thigh_rotor_com_z}"/>
+                <mass value="${thigh_rotor_mass}"/>
+                <inertia
+                    ixx="${thigh_rotor_ixx}" ixy="${thigh_rotor_ixy}" ixz="${thigh_rotor_ixz}"
+                    iyy="${thigh_rotor_iyy}" iyz="${thigh_rotor_iyz}"
+                    izz="${thigh_rotor_izz}"/>       
+            </inertial>
+        </link>
+
+        <joint name="${name}_calf_joint" type="revolute">
+            <origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
+            <parent link="${name}_thigh"/>
+            <child link="${name}_calf"/>
+            <axis xyz="0 1 0"/>
+            <dynamics damping="${damping}" friction="${friction}"/>
+            <limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_position_min}" upper="${calf_position_max}"/>
+        </joint>
+
+        <joint name="${name}_calf_rotor_joint" type="fixed">
+            <origin rpy="0 0 0" xyz="${calf_rotor_offset_x} ${calf_rotor_offset_y*mirror} 0"/>
+            <parent link="${name}_thigh"/>
+            <child link="${name}_calf_rotor"/>         
+        </joint>
+
+        <link name="${name}_calf">
+            <visual>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <mesh filename="file://$(find go1_description)/meshes/calf.dae" scale="1 1 1"/>
+                </geometry>
+                <!-- <material name="orange"/> -->
+            </visual>
+            <collision>
+                <origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/>
+                <geometry>
+                    <box size="${calf_length} ${calf_width} ${calf_height}"/>
+                </geometry>      
+            </collision>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
+                <mass value="${calf_mass}"/>
+                <inertia
+                    ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
+                    iyy="${calf_iyy}" iyz="${calf_iyz}"
+                    izz="${calf_izz}"/>       
+            </inertial>
+        </link>
+
+        <link name="${name}_calf_rotor">
+            <visual>
+                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <cylinder length="0.02" radius="0.035"/>
+                </geometry>
+                <material name="green"/>
+            </visual>
+            <!-- <collision>
+                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <cylinder length="0.02" radius="0.035"/>
+                </geometry>
+            </collision> -->
+            <inertial>
+                <origin rpy="0 0 0" xyz="${calf_rotor_com_x} ${calf_rotor_com_y} ${calf_rotor_com_z}"/>
+                <mass value="${calf_rotor_mass}"/>
+                <inertia
+                    ixx="${calf_rotor_ixx}" ixy="${calf_rotor_ixy}" ixz="${calf_rotor_ixz}"
+                    iyy="${calf_rotor_iyy}" iyz="${calf_rotor_iyz}"
+                    izz="${calf_rotor_izz}"/>     
+            </inertial>
+        </link>
+
+        <joint name="${name}_foot_fixed" type="fixed">
+            <origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
+            <parent link="${name}_calf"/>
+            <child link="${name}_foot"/>
+        </joint>
+
+        <link name="${name}_foot">
+            <visual>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <sphere radius="${foot_radius-0.01}"/>
+                </geometry>
+                <!-- <material name="orange"/> -->
+            </visual>
+            <collision>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <sphere radius="${foot_radius}"/>
+                </geometry>
+            </collision>
+            <inertial>
+                <mass value="${foot_mass}"/>
+                <inertia
+                    ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
+                    iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
+                    izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>             
+            </inertial>
+        </link>
+
+        <xacro:leg_transmission name="${name}"/>
+    </xacro:macro>
+</robot>
--- a/descriptions/go1_description/xacro/materials.xacro
+++ b/descriptions/go1_description/xacro/materials.xacro
@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<robot>
+
+    <material name="black">
+        <color rgba="0.0 0.0 0.0 1.0"/>
+    </material>
+
+    <material name="blue">
+        <color rgba="0.0 0.0 0.8 1.0"/>
+    </material>
+
+    <material name="green">
+        <color rgba="0.0 0.8 0.0 1.0"/>
+    </material>
+
+    <material name="grey">
+        <color rgba="0.2 0.2 0.2 1.0"/>
+    </material>
+
+    <material name="silver">
+        <color rgba="${233/255} ${233/255} ${216/255} 1.0"/>
+    </material>
+
+    <material name="orange">
+        <color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
+    </material>
+
+    <material name="brown">
+        <color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
+    </material>
+
+    <material name="red">
+        <color rgba="0.8 0.0 0.0 1.0"/>
+    </material>
+
+    <material name="white">
+        <color rgba="1.0 1.0 1.0 1.0"/>
+    </material>
+
+</robot>
--- a/descriptions/go1_description/xacro/robot.xacro
+++ b/descriptions/go1_description/xacro/robot.xacro
@ -0,0 +1,150 @@
+<?xml version="1.0"?>
+
+<robot name="go1" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:arg name="DEBUG" default="false"/>
+
+    <xacro:include filename="$(find go1_description)/xacro/const.xacro"/>
+    <xacro:include filename="$(find go1_description)/xacro/materials.xacro"/>
+    <xacro:include filename="$(find go1_description)/xacro/leg.xacro"/>
+    <xacro:include filename="$(find go1_description)/xacro/ros2_control.xacro"/>
+<!--    <xacro:include filename="$(find go1_description)/xacro/gazebo.xacro"/>-->
+    <xacro:include filename="$(find go1_description)/xacro/depthCamera.xacro"/>
+    <xacro:include filename="$(find go1_description)/xacro/ultraSound.xacro"/>
+
+    <!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->
+
+    <!-- Rotor related joint and link is only for demonstrate location. -->
+    <!-- Actually, the rotor will rotate and the joint is not fixed. Reduction ratio should be considered. -->
+
+    <!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
+    <xacro:if value="$(arg DEBUG)">
+        <link name="world"/>
+        <joint name="base_static_joint" type="fixed">
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <parent link="world"/>
+            <child link="base"/>
+        </joint>
+    </xacro:if> 
+
+    <link name="base">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size="0.001 0.001 0.001"/>
+            </geometry>
+        </visual>
+    </link>
+
+    <joint name="floating_base" type="fixed">
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <parent link="base"/>
+        <child link="trunk"/>
+    </joint>
+
+    <link name="trunk">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <mesh filename="file://$(find go1_description)/meshes/trunk.dae" scale="1 1 1"/>
+            </geometry>
+            <!-- <material name="orange"/> -->
+        </visual>
+        <collision>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size="${trunk_length} ${trunk_width} ${trunk_height}"/>
+            </geometry>
+        </collision>
+        <inertial>
+            <origin rpy="0 0 0" xyz="${trunk_com_x} ${trunk_com_y} ${trunk_com_z}"/>
+            <mass value="${trunk_mass}"/>
+            <inertia
+                ixx="${trunk_ixx}" ixy="${trunk_ixy}" ixz="${trunk_ixz}"
+                iyy="${trunk_iyy}" iyz="${trunk_iyz}"
+                izz="${trunk_izz}"/>  
+        </inertial>
+    </link>
+
+    <joint name="imu_joint" type="fixed">
+        <parent link="trunk"/>
+        <child link="imu_link"/>
+        <origin rpy="0 0 0" xyz="-0.01592 -0.06659 -0.00617"/>
+    </joint>
+
+    <link name="imu_link">
+        <inertial>
+            <mass value="0.001"/>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+        </inertial>
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size="0.001 0.001 0.001"/>
+            </geometry>
+            <!-- <material name="red"/> -->
+        </visual>
+        <collision>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size=".001 .001 .001"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <xacro:leg name="FR" mirror="-1" mirror_dae= "False" front_hind="1"  front_hind_dae="True"  />
+    <xacro:leg name="FL" mirror="1"  mirror_dae="True"   front_hind="1"  front_hind_dae="True"  />
+    <xacro:leg name="RR" mirror="-1" mirror_dae="False"  front_hind="-1" front_hind_dae="False" />
+    <xacro:leg name="RL" mirror="1"  mirror_dae="True"   front_hind="-1" front_hind_dae="False" />
+
+    <xacro:depthCamera camID="1" name="face">
+        <origin rpy="${PI} 0 0" xyz="0.2785 0.0125 0.0167"/>
+    </xacro:depthCamera>
+
+    <xacro:depthCamera camID="2" name="chin">
+        <origin rpy="${PI} ${PI/2} 0" xyz="0.2522 0.0125 -0.0436"/>
+    </xacro:depthCamera>
+
+    <xacro:depthCamera camID="3" name="left">
+        <origin rpy="${PI} 0.2618 ${PI/2}" xyz="-0.066 0.082 -0.0176"/>
+    </xacro:depthCamera>
+
+    <xacro:depthCamera camID="4" name="right">
+        <origin rpy="${PI} 0.2618 ${-PI/2}" xyz="-0.041 -0.082 -0.0176"/>
+    </xacro:depthCamera>
+
+    <xacro:depthCamera camID="5" name="rearDown">
+        <origin rpy="${PI} ${PI/2} 0" xyz="-0.0825 0.0125 -0.04365"/>
+    </xacro:depthCamera>
+
+    <joint name="camera_laserscan_joint_left" type="fixed">
+        <origin rpy="0 0.2618 0" xyz="0 0 0"/>
+        <parent link="camera_left"/>
+        <child link="camera_laserscan_link_left"/>
+    </joint>
+
+    <link name="camera_laserscan_link_left">
+    </link>
+
+    <joint name="camera_laserscan_joint_right" type="fixed">
+        <origin rpy="0 0.2618 0" xyz="0 0 0"/>
+        <parent link="camera_right"/>
+        <child link="camera_laserscan_link_right"/>
+    </joint>
+
+    <link name="camera_laserscan_link_right">
+    </link>
+
+    <xacro:ultraSound name="left">
+        <origin rpy="0 0.2618 ${PI/2}" xyz="-0.0535  0.0826 0.00868"/>
+    </xacro:ultraSound>
+
+    <xacro:ultraSound name="right">
+        <origin rpy="0 0.2618 ${-PI/2}" xyz="-0.0535 -0.0826 0.00868"/>
+    </xacro:ultraSound>
+
+    <xacro:ultraSound name="face">
+        <origin rpy="0 0 0" xyz="0.2747 0.0 -0.0088"/>
+    </xacro:ultraSound>
+</robot>
--- a/descriptions/go1_description/xacro/ros2_control.xacro
+++ b/descriptions/go1_description/xacro/ros2_control.xacro
@ -0,0 +1,156 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <ros2_control name="UnitreeSystem" type="system">
+
+        <hardware>
+            <plugin>hardware_unitree_mujoco/HardwareUnitree</plugin>
+        </hardware>
+
+        <joint name="FR_hip_joint">
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <command_interface name="effort"/>
+            <command_interface name="kp"/>
+            <command_interface name="kd"/>
+
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="FR_thigh_joint">
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <command_interface name="effort"/>
+            <command_interface name="kp"/>
+            <command_interface name="kd"/>
+
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="FR_calf_joint">
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <command_interface name="effort"/>
+            <command_interface name="kp"/>
+            <command_interface name="kd"/>
+
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="FL_hip_joint">
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <command_interface name="effort"/>
+            <command_interface name="kp"/>
+            <command_interface name="kd"/>
+
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="FL_thigh_joint">
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <command_interface name="effort"/>
+            <command_interface name="kp"/>
+            <command_interface name="kd"/>
+
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="FL_calf_joint">
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <command_interface name="effort"/>
+            <command_interface name="kp"/>
+            <command_interface name="kd"/>
+
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="RR_hip_joint">
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <command_interface name="effort"/>
+            <command_interface name="kp"/>
+            <command_interface name="kd"/>
+
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="RR_thigh_joint">
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <command_interface name="effort"/>
+            <command_interface name="kp"/>
+            <command_interface name="kd"/>
+
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="RR_calf_joint">
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <command_interface name="effort"/>
+            <command_interface name="kp"/>
+            <command_interface name="kd"/>
+
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="RL_hip_joint">
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <command_interface name="effort"/>
+            <command_interface name="kp"/>
+            <command_interface name="kd"/>
+
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="RL_thigh_joint">
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <command_interface name="effort"/>
+            <command_interface name="kp"/>
+            <command_interface name="kd"/>
+
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="RL_calf_joint">
+            <command_interface name="position"/>
+            <command_interface name="velocity"/>
+            <command_interface name="effort"/>
+            <command_interface name="kp"/>
+            <command_interface name="kd"/>
+
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+    </ros2_control>
+
+</robot>
--- a/descriptions/go1_description/xacro/transmission.xacro
+++ b/descriptions/go1_description/xacro/transmission.xacro
@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="leg_transmission" params="name">
+
+        <transmission name="${name}_hip_tran">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${name}_hip_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${name}_hip_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+        <transmission name="${name}_thigh_tran">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${name}_thigh_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${name}_thigh_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+        <transmission name="${name}_calf_tran">
+            <type>transmission_interface/SimpleTransmission</type>
+            <joint name="${name}_calf_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            </joint>
+            <actuator name="${name}_calf_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+            </actuator>
+        </transmission>
+
+    </xacro:macro>
+
+</robot>
--- a/descriptions/go1_description/xacro/ultraSound.xacro
+++ b/descriptions/go1_description/xacro/ultraSound.xacro
@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:macro name="ultraSound" params="name *origin">
+        <joint name="ultraSound_joint_${name}" type="fixed">
+            <xacro:insert_block name="origin"/>
+            <parent link="trunk"/>
+            <child link="ultraSound_${name}"/>
+        </joint>
+        <link name="ultraSound_${name}">
+            <collision>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <box size=".001 .001 .001"/>
+            </geometry>
+            </collision>
+
+            <visual>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <mesh filename="file://$(find go1_description)/meshes/ultraSound.dae" scale="1 1 1"/>
+            </geometry>
+            <material name="black"/>
+            </visual>
+
+            <inertial>
+            <mass value="1e-5" />
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
+            </inertial>
+        </link>
+    </xacro:macro>
+</robot>
--- a/descriptions/go2_description/config/robot_control.yaml
+++ b/descriptions/go2_description/config/robot_control.yaml
@ -1,7 +1,7 @@
 # Controller Manager configuration
 controller_manager:
  ros__parameters:
-    update_rate: 50  # Hz
+    update_rate: 200  # Hz
    use_sim_time: true  # If running in simulation

    # Define the available controllers
--- a/descriptions/go2_description/launch/gazebo.launch
+++ b/descriptions/go2_description/launch/gazebo.launch
@ -1,20 +0,0 @@
-<launch>
-  <include
-    file="$(find gazebo_ros)/launch/empty_world.launch" />
-  <node
-    name="tf_footprint_base"
-    pkg="tf"
-    type="static_transform_publisher"
-    args="0 0 0 0 0 0 base_link base_footprint 40" />
-  <node
-    name="spawn_model"
-    pkg="gazebo_ros"
-    type="spawn_model"
-    args="-file $(find go2_description)/urdf/go2_description.urdf -urdf -model go2_description"
-    output="screen" />
-  <node
-    name="fake_joint_calibration"
-    pkg="rostopic"
-    type="rostopic"
-    args="pub /calibrated std_msgs/Bool true" />
-</launch>
--- a/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp
+++ b/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp
@ -34,7 +34,6 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa
        }
    }

-    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Initializing Unitree SDK2 Channel");
    ChannelFactory::Instance()->Init(1, "lo");
    lowcmd_publisher =
            std::make_shared<ChannelPublisher<unitree_go::msg::dds_::LowCmd_> >(
@ -51,7 +50,6 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa
        },
        1);
    init_low_cmd();
-    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Initialized Unitree SDK2 Channel");


    return SystemInterface::on_init(info);