Merge pull request #76 from Caltech-AMBER/achilles-model

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];
@@ -545,7 +551,7 @@ namespace obelisk {
                         //  If the velocity sensor ordering does not match the position sensors, then their joint names
                         //  will not align.
                         int sensor_addr = this->model_->sensor_adr[sensor_id];
-                        if (mj_sensor_types.at(i) == "joint_pos") {
+                        if (mj_sensor_types.at(i) == "jointpos") {
                             msg.joint_pos.emplace_back(this->data_->sensordata[sensor_addr]);
                             int joint_id = this->model_->sensor_objid[sensor_id];
                             if (joint_id == -1) {
@@ -555,7 +561,7 @@ namespace obelisk {
                             } else {
                                 msg.joint_names.emplace_back(this->model_->names + this->model_->name_jntadr[joint_id]);
                             }
-                        } else if (mj_sensor_types.at(i) == "joint_vel") {
+                        } else if (mj_sensor_types.at(i) == "jointvel") {
                             msg.joint_vel.emplace_back(this->data_->sensordata[sensor_addr]);
                         } else {
                             RCLCPP_ERROR_STREAM(

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/python/obelisk_py/core/sim/mujoco.py

@@ -108,14 +108,14 @@ def timer_callback() -> None:
                         self.get_logger().error(f"Sensor {sensor_name} not found!")
                         raise ValueError(f"Sensor {sensor_name} not found!")
 
-                    if obk_sensor_field == "joint_pos":
+                    if obk_sensor_field == "jointpos":
                         joint_pos.append(self.shared_sensordata[sensor_adr])
 
                         # getting joint name from the joint position sensor
                         joint_id = self.mj_model.sensor_objid[sensor_id]
                         joint_names.append(self.mj_model.joint(joint_id).name)
 
-                    elif obk_sensor_field == "joint_vel":
+                    elif obk_sensor_field == "jointvel":
                         joint_vel.append(self.shared_sensordata[sensor_adr])
 
                     else:

obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml

@@ -85,8 +85,8 @@ onboard:
             dt: 0.001
             msg_type: ObkJointEncoders
             sensor_names:
-              joint_pos: joint_pos
-              joint_vel: joint_vel
+              joint_pos: jointpos
+              joint_vel: jointvel
           - topic: /obelisk/dummy/imu
             dt: 0.002
             msg_type: ObkImu

obelisk_ws/src/obelisk_ros/config/dummy_cpp_viz.yaml

@@ -83,8 +83,8 @@ onboard:
             dt: 0.001
             msg_type: ObkJointEncoders
             sensor_names:
-              joint_pos: joint_pos
-              joint_vel: joint_vel
+              joint_pos: jointpos
+              joint_vel: jointvel
           - topic: /obelisk/dummy/imu
             dt: 0.002
             msg_type: ObkImu

obelisk_ws/src/obelisk_ros/config/dummy_py.yaml

@@ -76,8 +76,8 @@ onboard:
             dt: 0.001
             msg_type: ObkJointEncoders
             sensor_names:
-              joint_pos: joint_pos
-              joint_vel: joint_vel
+              joint_pos: jointpos
+              joint_vel: jointvel
           - topic: /obelisk/dummy/imu
             dt: 0.002
             msg_type: ObkImu

obelisk_ws/src/obelisk_ros/config/leap_cpp.yaml

@@ -92,22 +92,22 @@ onboard:
             dt: 0.002
             msg_type: ObkJointEncoders
             sensor_names:
-              if_mcp_sensor: joint_pos
-              if_rot_sensor: joint_pos
-              if_pip_sensor: joint_pos
-              if_dip_sensor: joint_pos
-              mf_mcp_sensor: joint_pos
-              mf_rot_sensor: joint_pos
-              mf_pip_sensor: joint_pos
-              mf_dip_sensor: joint_pos
-              rf_mcp_sensor: joint_pos
-              rf_rot_sensor: joint_pos
-              rf_pip_sensor: joint_pos
-              rf_dip_sensor: joint_pos
-              th_cmc_sensor: joint_pos
-              th_axl_sensor: joint_pos
-              th_mcp_sensor: joint_pos
-              th_ipl_sensor: joint_pos
+              if_mcp_sensor: jointpos
+              if_rot_sensor: jointpos
+              if_pip_sensor: jointpos
+              if_dip_sensor: jointpos
+              mf_mcp_sensor: jointpos
+              mf_rot_sensor: jointpos
+              mf_pip_sensor: jointpos
+              mf_dip_sensor: jointpos
+              rf_mcp_sensor: jointpos
+              rf_rot_sensor: jointpos
+              rf_pip_sensor: jointpos
+              rf_dip_sensor: jointpos
+              th_cmc_sensor: jointpos
+              th_axl_sensor: jointpos
+              th_mcp_sensor: jointpos
+              th_ipl_sensor: jointpos
       timers:
         - ros_parameter: timer_sensor_setting
           timer_period_sec: 0.02

obelisk_ws/src/obelisk_ros/config/leap_py.yaml

@@ -84,22 +84,22 @@ onboard:
             dt: 0.002
             msg_type: ObkJointEncoders
             sensor_names:
-              if_mcp_sensor: joint_pos
-              if_rot_sensor: joint_pos
-              if_pip_sensor: joint_pos
-              if_dip_sensor: joint_pos
-              mf_mcp_sensor: joint_pos
-              mf_rot_sensor: joint_pos
-              mf_pip_sensor: joint_pos
-              mf_dip_sensor: joint_pos
-              rf_mcp_sensor: joint_pos
-              rf_rot_sensor: joint_pos
-              rf_pip_sensor: joint_pos
-              rf_dip_sensor: joint_pos
-              th_cmc_sensor: joint_pos
-              th_axl_sensor: joint_pos
-              th_mcp_sensor: joint_pos
-              th_ipl_sensor: joint_pos
+              if_mcp_sensor: jointpos
+              if_rot_sensor: jointpos
+              if_pip_sensor: jointpos
+              if_dip_sensor: jointpos
+              mf_mcp_sensor: jointpos
+              mf_rot_sensor: jointpos
+              mf_pip_sensor: jointpos
+              mf_dip_sensor: jointpos
+              rf_mcp_sensor: jointpos
+              rf_rot_sensor: jointpos
+              rf_pip_sensor: jointpos
+              rf_dip_sensor: jointpos
+              th_cmc_sensor: jointpos
+              th_axl_sensor: jointpos
+              th_mcp_sensor: jointpos
+              th_ipl_sensor: jointpos
       timers:
         - ros_parameter: timer_sensor_setting
           timer_period_sec: 0.02

obelisk_ws/src/robots/achilles_model/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.16)
+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.