Achilles model

obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h

@@ -53,6 +53,8 @@ namespace obelisk {
                 }
             }
 
+            RCLCPP_INFO_STREAM(this->get_logger(), "XML Path:" << xml_path_);
+
             nu_                = GetNumInputs(mujoco_config_map); // Required
             time_step_         = GetTimeSteps(mujoco_config_map);
             num_steps_per_viz_ = GetNumStepsPerViz(mujoco_config_map);
@@ -519,6 +521,10 @@ namespace obelisk {
                     std::lock_guard<std::mutex> lock(sensor_data_mut_);
                     int sensor_id = mj_name2id(this->model_, mjOBJ_SENSOR, sensor_names.at(i).c_str());
                     if (sensor_id == -1) {
+                        RCLCPP_ERROR_STREAM_ONCE(
+                            this->get_logger(),
+                            "Sensor not found in Mujoco! Make sure your XML has the sensor. Sensor name: "
+                                << sensor_names.at(i));
                         throw std::runtime_error("Sensor not found in Mujoco! Make sure your XML has the sensor.");
                     }
                     int joint_id   = this->model_->sensor_objid[sensor_id];

obelisk/cpp/viz/include/obelisk_viz_robot_default.h

@@ -36,10 +36,10 @@ namespace obelisk::viz {
                 this->base_tf_.transform.translation.y = 0.0;
                 this->base_tf_.transform.translation.z = 0.0;
 
-                this->base_tf_.transform.rotation.x    = 0.0;
-                this->base_tf_.transform.rotation.y    = 0.0;
-                this->base_tf_.transform.rotation.z    = 0.0;
-                this->base_tf_.transform.rotation.w    = 1.0;
+                this->base_tf_.transform.rotation.x = 0.0;
+                this->base_tf_.transform.rotation.y = 0.0;
+                this->base_tf_.transform.rotation.z = 0.0;
+                this->base_tf_.transform.rotation.w = 1.0;
             } else if (msg.q_base.size() == 7) {
                 this->base_tf_.transform.translation.x = msg.q_base.at(0);
                 this->base_tf_.transform.translation.y = msg.q_base.at(1);
@@ -61,18 +61,18 @@ namespace obelisk::viz {
             // ----------- Create the JointState message ----------- //
             // Verify the message is self consistent
             if (msg.q_joints.size() != msg.joint_names.size()) {
-                RCLCPP_ERROR_STREAM(this->get_logger(),
-                                    "The message's q_joint and joint_names vectors have different lengths! "
-                                        << "Number of joint values: " << msg.q_joints.size()
-                                        << ". Number of joint names:" << msg.joint_names.size());
+                RCLCPP_ERROR_STREAM_ONCE(this->get_logger(),
+                                         "The message's q_joint and joint_names vectors have different lengths! "
+                                             << "Number of joint values: " << msg.q_joints.size()
+                                             << ". Number of joint names:" << msg.joint_names.size());
             }
 
             // Verify the message against the URDF
             if (msg.joint_names.size() != this->model_.joints_.size()) {
-                RCLCPP_ERROR_STREAM(this->get_logger(),
-                                    "The message's number of joints does match the URDF! "
-                                        << "Number of joint names in the message: " << msg.joint_names.size()
-                                        << ". Number of joint names in the URDF:" << this->model_.joints_.size());
+                RCLCPP_ERROR_STREAM_ONCE(this->get_logger(),
+                                         "The message's number of joints does match the URDF! "
+                                             << "Number of joint names in the message: " << msg.joint_names.size()
+                                             << ". Number of joint names in the URDF:" << this->model_.joints_.size());
             }
 
             for (const auto& msg_joint : msg.joint_names) {
@@ -83,10 +83,10 @@ namespace obelisk::viz {
                     }
                 }
                 if (!valid_name) {
-                    RCLCPP_ERROR_STREAM(this->get_logger(),
-                                        "At least one joint name in the message does not match the URDF! "
-                                            << "The joint name: " << msg_joint
-                                            << " found in the message does not match a joint name in the URDF!");
+                    RCLCPP_ERROR_STREAM_ONCE(this->get_logger(),
+                                             "At least one joint name in the message does not match the URDF! "
+                                                 << "The joint name: " << msg_joint
+                                                 << " found in the message does not match a joint name in the URDF!");
                 }
             }
 

obelisk_ws/src/robots/achilles_model/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.8)
+project(achilles_model)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+
+# install all the mesh files
+install(
+  DIRECTORY meshes/
+  DESTINATION share/${PROJECT_NAME}/meshes
+)
+
+# install the urdf
+install(
+  DIRECTORY urdf/
+  DESTINATION share/${PROJECT_NAME}/urdf
+)
+
+# install the mujoco xml
+install(
+  DIRECTORY mujoco/
+  DESTINATION share/${PROJECT_NAME}/mujoco
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

obelisk_ws/src/robots/achilles_model/LICENSE

@@ -0,0 +1,17 @@
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.